適用:XC330 適用:全シリーズ 適用:全シリーズ 適用:全シリーズ 適用:XM/XH/XDシリーズ 適用:全シリーズ
¶>XC330
¶オイルシールがないためグリスがリークする。
ROBOTIS曰く問題無いので拭けとの事。
XC330 全シリーズ
全シリーズ 全シリーズ
¶搭載LDOの絶対最大定格を超えた場合に故障して発熱。現行品は対策品が適用済み。
適用:XC330
Attach file: E212_3D.png by takaboo
OpenOCD_x86_0.12.0+dev-00524-g07141132a
OpenOCD_x64_0.12.0+dev-00524-g07141132a
Attach file: openocd.pdf by takaboo
¶Attach file: openocd.pdf by takaboo
¶OpenOCD_x86_0.12.0+dev-00519-gb6ee13720
OpenOCD_x64_0.12.0+dev-00519-gb6ee13720
OpenOCD_x86_0.12.0+dev-00516-gefdd5e09b
OpenOCD_x64_0.12.0+dev-00516-gefdd5e09b
オープンソースとして公開されている各種JTAG-ICEに対応したデバッガです。マイコン上のプログラムをデバッグする事だけが目的というわけではなく、マイコンに接続ないし内蔵された様々なメモリへのアクセスも出来ますので、単体でマイコン内蔵のFLASH書き込みツールとしても便利に使えます。
ここではARM7に主眼を置いていますが、OpenOCDとしてはARM7 (ARM7TDMI, ARM720t), ARM9 (ARM920T, ARM922T, ARM926EJ-S, ARM966E-S)・XScale (PXA25x, IXP42x)・Cortex-M3 (Stellaris LM3, ST STM32F1, ST STM32F2, ATMEL SAM3等)のデバッグ、CFI compatible NOR フラッシュ (Intel, AMD/Spansion)・各種マイコン内蔵フラッシュ (LPC2000, LPC1700, AT91SAM7, SAM3, SAM4, STR7x, STR9x, LM3, STM32F等)の書き込み等をサポートしますので、興味があれば本家のサイトを参考に使ってみてはいかがでしょう。
OpenOCDによって構成できるシステムの概略イメージです。
OpenOCDはPC内でネットワークのデーモンとして実行され、割り当てられたポートに対して種々のアプリケーションからアクセスし、OpenOCD及びJTAG I/F Hardwareを介してターゲットとコンタクトする形を取るのが一般的の様です。
*コンパイル済みバイナリの提供 [#w4125957]
OpenOCD自体はソースから使用者自ら構築するのが通常ですが、TINY JTAG-ICE2を有効に活用するためにWindows上で動作するバイナリを提供しています。I/FないしチップはTINY JTAG-ICE2, J-LINK, FT2232, ST-Link, CMSIS-DAPに限定し、以下の様な操作でMSYS2上でコンパイルしています。
# git clone git://git.code.sf.net/p/openocd/code openocd # cd openocd # make distclean # git pull origin master # ./guess-rev.sh # ./bootstrap # ./configure --build=mingw32 --host=mingw32 --disable-dependency-tracking --disable-werror --enable-dummy --enable-jtag_vpi --enable-remote-bitbang --enable-amtjtagaccel --enable-gw16012 --enable-parport --enable-parport-giveio --enable-usbprog --enable-jlink --enable-stlink --enable-usb_blaster_libftdi # make
OpenOCD_x86_0.12.0+dev-00524-g07141132a
OpenOCD_x64_0.12.0+dev-00524-g07141132a
OpenOCD_x86_0.12.0+dev-00519-gb6ee13720
OpenOCD_x64_0.12.0+dev-00519-gb6ee13720
OpenOCD_x86_0.12.0+dev-00516-gefdd5e09b
OpenOCD_x64_0.12.0+dev-00516-gefdd5e09b
OpenOCD_x86_0.11.0+dev-00873-g219cb9598
OpenOCD_x64_0.11.0+dev-00873-g219cb9598
OpenOCD_x86_0.10.0+dev-01524-g861e75f54
OpenOCD_x64_0.10.0+dev-01524-g861e75f54
OpenOCD_0.9.0-dev-00874-g2d4ae3f-ft2232only
OpenOCD_0.8.0-rc2-dev-00456-g7ad635b-ft2232only.zip
※0.10.0-rc1以降のバイナリからWinUSBベースに変更したので、デバイスドライバはこちらの手順でインストールの事
市販品は星の数ほどありますが、似た回路で自作するのも有りです。最新のデバイスドライバ等の詳細はTINY JTAG-ICE2のページで確認してください。
WinUSBのデバイスドライバに入れ替える必要があります。
以前紹介していたlibusb-win32のドライバに入れ替える方法ではなく、Windows上であればこちらもST-LINKと同様にWinUSBのデバイスドライバに入れ替える事で対応します。
ターゲットによって使用方法が異なる部分が多いため、ドキュメントを参考にしてもらうのがベストです。ここで一生懸命日本語訳しても、あっという間に陳腐化して全く使い物になる代物ですので。
動かすまでの大まかなネタは、
Attach file: E179B_CN5-12 by takaboo
¶Attach file: E179B_CN4.png by takaboo
¶Attach file: E179B_CN3 by takaboo
¶Attach file: E179B_CN2.png by takaboo
DXHATはRaspberry Piシリーズ用のHATで、装備した主な機能は以下の通りです。
※BTE100の後継機です。
※本製品は半田付け作業を要求します。
※Raspberry Pi 3/4/5と電源装置が別途必要です。
品名 | 数量 | 型番・その他 |
DXHAT | 1個 | E179B |
スペーサ・ネジ類 | 1式 | スペーサー六角両メネジ M2.6 L8 x4 スペーサー 丸型中空 M2.6 L9 x4 スペーサー 丸型中空 M2.6 L8 x4 なべ小ねじ M2.6 L15 x4 低頭小ねじ M2.6 L5 x4 |
ターミナルブロック | 1個 | TB111-2-2-U-1-1 |
商品番号 | BTE100 | |
PCBリビジョン | E179 | |
動作温度範囲 | 0~70℃ 結露なき事 | |
寸法 | 60x56.5x15mm 取り付け穴:58x49mm 4-φ3.0mm | |
重量 | 25g | |
外部電源 | DC6.5~24V (絶対最大定格 DC-0.3V/+30V) | |
内部電源 | DC5V | 外部電源からDCDCコンバータを介して生成 最大6A供給可 |
DC3.3V | Raspberry Piより供給 | |
RTC | DS3231M リチウム二次電池による簡易電源バックアップ | |
冷却ファン | 電源電圧 | DC5V |
最大送風量 | 2.2CFM | |
各I/F仕様 | RS-485 | 複信 半二重 最大通信速度 20Mbps 最大端末数 256台 ESD保護 ±15kV 絶対最大定格 -60~60V |
TTL | 複信 半二重 最大通信速度 12Mbps 最大端末数 32台 ESD保護 ±2kV 絶対最大定格 -0.5~6V | |
コネクタ等 | 2x20ピン パススルーソケット x1 2ピン5.08mmピッチランド x1 JST S4B-EH x4 JST S3B-EH x4 JST B2B-ZR-SM4 x1 4ピン2.5mmピッチランド x3 | |
環境配慮 | RoHS準拠, 鉛フリー半田 |
端子番号 | 信号名 |
1(N) | GND |
2(P) | VIN |
端子番号 | 信号名 |
1 | GND |
2 | PB |
3 | - |
Pats Name | JST Parts Number |
基板用ヘッダー | B3B-ZR |
ハウジング | ZHR-3 |
ターミナル | SZH-002x-P0.5 |
圧接コネクタ | 03ZR-8M-P |
No. | 端子名称 | I/O | No. | 端子名称 | I/O |
1 | 3V3 | I | 2 | 5V | O |
3 | GPIO2(SDA1) | I/O | 4 | 5V | O |
5 | GPIO3(SCL1) | I/O | 6 | GND | O |
7 | GPIO4(KILL) | O | 8 | GPIO14(TXD0/1) | I |
9 | GND | O | 10 | GPIO15(RXD0/1) | O |
11 | GPIO17(INT) | I | 12 | - | |
13 | - | 14 | GND | O | |
15 | - | 16 | - | ||
17 | 3V3 | I | 18 | - | |
19 | - | 20 | GND | O | |
21 | GPIO9(RXD4) | O | 22 | - | |
23 | - | 24 | GPIO8(TXD4) | I | |
25 | GND | O | 26 | - | |
27 | GPIO0(TXD2) | I | 28 | GPIO1(RXD2) | O |
29 | I/O | 30 | GND | O | |
31 | - | 32 | GPIO12 | I/O | |
33 | GPIO13 | I/O | 34 | GND | O |
35 | GPIO19(FAN) | I/O | 36 | GPIO16 | I/O |
37 | GPIO26 | I/O | 38 | GPIO20 | I/O |
39 | GND | O | 40 | - |
Pats Name | JST Parts Number |
基板用ヘッダー | B2B-ZR-SM4 |
ハウジング | ZHR-2 |
ターミナル | SZH-002x-P0.5 |
圧接コネクタ | 02ZR-8M-P |
端子番号 | 信号名 |
1 | 5V |
2 | オープンドレイン出力 |
端子番号 | 信号名 |
1 | GND |
2 | VDD |
3 | TTL Signal |
端子番号 | 信号名 |
1 | GND |
2 | VDD |
3 | RS-485 D+ |
4 | RS-485 D- |
Pats Name | JST Parts Number |
基板用ヘッダー | SM04B-SRSS-TB |
ハウジング | SHR-04V-S |
ターミナル | SSH-003T-P0.2-H |
No. | 端子名称 (Pico) |
1 | GND |
2 | COM (OUT) |
3 | R_SDA |
4 | R_SCL |
端子番号 | 信号名 |
1 | 5V |
2 | COM |
3 | 3V3 |
4 | GND |
端子番号 | 信号名 |
1 | 3V3 |
2 | GND |
3 | GPIO12(PWM0/SD4/DPI_D8/SPI5_CE0_N/TXD5/SDA5) |
4 | GPIO13(PWM1/SD5/DPI_D9/SPI5_MISO/RXD5/SCL5) |
端子番号 | 信号名 |
1 | GPIO19(PCM_FS/SD11/DPI_D15/SPI6_MISO/SPI1_MISO/PWM1) |
2 | GPIO16(FL0/SD8/DPI_D12/CTS0/SPI1_CE2_N/CTS1) |
3 | GPIO26(SD0_DAT2/TE0/DPI_D22/SD1_DAT2/ARM_TDI/SPIO5_CE1_N) |
4 | GPIO20(PCM_DIN/SD12/DPI_D16/SPI6_MOSI/SPI1_MOSI/GPCLK0) |
Raspberry Piに適用するOSや環境の構築状況によって設定方法や挙動が変わります。ここではそれらの詳説を省略しますので、公式サイト等などから情報を補完して下さい。
HATの各機能は主にconfig.txtに必要な情報を追記する事で活性化されます。なおconfig.txtから別のファイルをincludeしている場合もありますので、それらも含めて全ての設定ファイルを精査する必要があります。
CN3と四隅の取り付け穴の位置をRaspberry Piのピンヘッダと取り付け穴に一致させ装着します。
HATとRaspberry Piの電子部品と基板が接触しないよう四隅の取り付け穴に同じ高さのスペーサを設置し、相互の基板を平行に保持します。
なお実測でRapspberry Pi 4の場合は8mm、Raspberry Pi 3の場合は9mm以上のスペーサが必要ですが、10mm以下でないとピンヘッダが嵌合しません。
CN1にコネクタやケーブルを装備し外部電源を供給しますが、CN1から外部電源を供給してもRaspberry Piへの給電は開始されず、CN2に装着するプッシュモーメンタリなスイッチで電源供給の操作を行う必要があります。
Raspberry Pi上でOSが動作している間に電源の供給が突然断たれる事は大抵の場合において想定されておらず、一般的にはpoweroffコマンドを実行し、シャットダウン処理が完全に完了した頃合いを見計らって電源の供給を断つといった操作が強いられます。もしOSが動作している時に突如電源の供給が断たれてしまうと、タイミングによってはファイルシステムが破損し、次回以降OSが正常に起動しなくなります。
HATには電源の操作の煩雑さを簡素化する機能が備わっており、CN2に装備したプッシュスイッチの操作によって、電源ON・OSへのシャットダウン励起/OSの終了と同時に電源OFF・強制電源OFFといった処理が安全に行えます。
config.txtに追記する情報
[all] dtoverlay=gpio-shutdown,gpio_pin=17 dtoverlay=gpio-poweroff,gpiopin=4,active_low=1
なおubuntuをGUIで起動している場合は、プッシュスイッチを押下するとログアウトのプロンプトが表示され、タイムアウトするかダイアログのpower offを選択するまでシャットダウンが行われません。プッシュスイッチ操作によってシンプルにシャットダウン処理を行わせるには、GNOMEの設定を変更する必要があります。なおログインしていない状態ではこの設定は効果がありません。
gsettings set org.gnome.SessionManager logout-prompt false
ちなみに付属の電源ボタンモジュールは3線で構成されていますが、電源のON/OFFだけであればPBとGND信号の2本あれば事足ります。残りの1本は1kΩの抵抗を介してプッシュスイッチに内蔵されているLEDのアノードに接続されていますので、CN2-3に5Vや3.3V程度の電圧やGPIOの信号を接続するとLEDを点灯させる事ができます。
外部電源の逆極性への耐性はないため、絶対に極性を間違えてはならない。 HATを装着した後は、Raspberry Pi本体に備わっているMicro USBないしUSB Type Cコネクタから電源を供給してはならない。 |
HATに装備された冷却ファンはRaspberry Piへ外気を送り込む事で冷却を促し、発熱によってSoCのクロックが下がりパフォーマンスが低下する事を低減させるのが目的です。
合わせてRaspberry Pi上の主要チップにヒートシンクを装着すれば、冷却効果を更に高める事が期待できます。
エアフローはHAT上面からファンで吸気し、HATとRaspberry Piの間隙から排気を想定しています。もしケースなどに封入する際は、吸気口と排気口を適宜設けたり、HAT上の冷却ファンを取り外してケース側でエアフローを設計して下さい。
冷却ファンのON/OFFはRaspberry PiのGPIO19を用いて行われ、内部温度と設定温度に応じてON/OFFを自動的に制御する事ができます。
config.txtに追記する情報
[all] dtoverlay=gpio-fan,gpiopin=19,temp=50000temp=50000は50℃で冷却ファンをONする意味で、摂氏度の1000倍の値を指定
設定温度を10℃下回るとファンはOFFする
なおRaspberry Piの発熱はOSや実行しているアプリケーションの負荷等に依存します。またRaspberry Piの性能限界までオーバークロックすると冷却しきれない場合があります。
冷却ファンへの接触防止措置を行っていないため、巻き込まれ等に注意。 |
Raspberry Piの時刻はNTPサーバなどを利用してOSの起動時に更新するのが一般的なようです。しかし時刻を取得できなければ、OSは不正確な時刻を元に動作する事になります。
HATはI2C接続のRTC(I2Cスレーブアドレス0x68)を装備しており、外部電源が断たれた場合でも数日間はバッテリがRTCの電源を供給します。バッテリを満充電させるには、少なくとも半日程度は電源ON状態を維持して下さい。
config.txtに追記する情報
[pi5] dtparam=rtc=off [all] dtparam=i2c_arm=on dtoverlay=i2c-rtc,ds3231
hwclockコマンドを使う事でRTCが機能しているかの確認ができます。
#print hardware clock hwclock -r #set the hardware clock from the current system time hwclock -w #set the system time from the hardware clock hwclock -s
なおubuntuであればNTCとRTCそしてシステムクロックがうまく連動しますが、他のOSの場合は初期状態においてRTCの扱いが異なる場合があります。思ったような挙動をしない場合は、こちらの情報を参考にして下さい。
Raspberry PiのUSBポートにDXHUB等を接続してDynamixel用のI/Fを増設する事も可能ですが、HATにはDynamixelと通信するためのRS-485とTTLの2種類のI/F(CN5~8/CN9~12)が装備されており、CN1に供給される電源はDynamiel用の電源として供給されます。
Raspberry Piが持っているUARTを使用する事で、USB接続したシリアルI/Fの一般的な1msのレイテンシを超越した運用ができます。基本的にはTTL2DXIFと同等の回路が装備されていますので、注意事項はそれと同様となります。
使用するUARTはJP1, JP2を半田ジャンパする事で選択しますが、Raspberry PiのGPIOは複数の機能を兼用しているため、使用環境に応じて適宜設定します。またUARTのベースクロックを64MHzに設定する事で、最大4Mbpsの通信速度に対応できます。
config.txtに追記する情報
[pi3] init_uart_clock=64000000 dtoverlay=uart0=on dtoverlay=pi3-miniuart-bt [pi4] init_uart_clock=64000000 #UART1を活性化 dtoverlay=uart1,txd1_pin=14,rxd1_pin=15 #UART2を活性化 dtoverlay=uart2,txd2_pin=0,rxd2_pin=1 #UART4を活性化 dtoverlay=uart4,txd4_pin=8,rxd4_pin=9使用するポートのみを活性化する事を推奨。Linux上でのデバイス名は概ねttyAMAのプレフィクスで始まる。
またRaspberry Pi 3/Zeroの場合はcmdline.txtに記述されている「console=serial0,115200」を削除する事。
対応するデバイスは以下の通りです。
Device Name |
RS-485ないしTTL I/Fを装備したDynamixel DX・AX・RX・EX・MX・X・PRO・P・USS3・DXMIO |
なおMolex社のコネクタが装備されたデバイスを接続する場合は、Robot Cable-X3P 180mm (Convertible)ないしRobot Cable-X4P 180mm (Convertible)を使用して下さい。
外部電源の電流容量が小さいとDynamixelに過度な負荷が生じた際にRaspberry Piに供給される電源に影響が及ぶ。 Dynamixelから発生した逆起電力によって電源電圧が上昇た場合、HAT上の電子部品の絶対最大定格電圧を超えて破損する可能性がある。 接続するDynamixelの総容量は、外部電源のラインに挿入されたFETとFR-4のガラスエポキシ基板の許容電流容量を超えない事。 様々な事象の想定範囲を広げ、不慮の故障に至らないようシステムを設計する事。 |
Linux上で動作するDynamixelを制御するプログラムを作る際は、別途用意したライブラリを用いる事でシリアル通信やプロトコルを意識せずにコーディングができます。
APIに対して対象デバイスのID・番地・値といった引数を与えるだけでコントロールテーブルの読み書きが実現でき、マルチスレッド等で複数のデバイスに対して個別にアクセスする様なアプリケーションも容易に構成できます。
Dynaimxelには2種類のプロトコルが存在しますが相互非互換であるため、プロトコルに応じて2種類のライブラリを用意しています。
Dynamixel Protocol V2用ライブラリを使用する際の手順を少しだけ紹介しておきます。全てターミナル上での操作となります。
sudo apt install build-essential unzip
cd ~ wget https://www.besttechnology.co.jp/download/DX2LIB_V2.9.zip unzip DX2LIB_V2.9.zip
cd ~/DX2LIB_v2.9/DX2LIB/ bash ./build_dx2lib.sh
以上でライブラリファイルが自身の環境用に再構築され、同時に各サンプルソースディレクトに実行に必要なファイルがコピーされます。
Pythonで記述されたサンプルソースであれば多少の改変で動かせますので試してみます。
cd ~/DX2LIB_v2.9/SampleCode/Python/ nano smpl1.py
通信ポートとボーレートは自身の環境に依存するため、適宜修正して保存します。例としてuartが「/dev/ttyAMA1」、ボーレートが「2Mbps」、IDが「1」とした場合は、ソースコード中の以下の変数に代入している部分を修正して保存します。
COMPort = b'/dev/ttyAMA1' Baudrate = 2000000 TargetID = 1
最後にsmpl1.pyをPythonで読み込ませて実行させます。
python3 smpl1.py
もしpermission deniedと言われた場合は、adduserを使ってdialoutグループに自身のユーザーを登録してから実行し直しましょう。
sudo adduser $USER dialout
Dynamixelとの通信に成功すると、DynamixelのLEDが1秒間点灯します。他のサンプルソースもほぼ同様の変更を加えることで利用できます。
なお「~/DX2LIB_v?.?/SampleCode/C/config.c」はID・ボーレート・動作モードの変更、テスト運転用のサンプルソースになりますので、Dynamixelの諸設定変更の際は利用してみてはいかがでしょうか。
またRaspberry Piの動作条件によってはAPIがタイムアウトエラーを返す場合があります。その際はDX2_SetTimeOutOffset APIを使ってタイムアウトエラーが発生しない程度の時間を設定すると良いでしょう。
I2CバスにはRTCが既に接続されていますが、I2C接続のデバイスをさらに追加できるようにバスリピータを介してJ2にI2Cバスが接続されています。
I2Cバスに接続する外部機器の電圧に応じて、J1のCOMに供給する電源を5Vか3.3Vから選択します。同時にI2Cバスの信号ラインは4.7kΩの抵抗を介して選択したCOMの電圧にプルアップされます。
なおJ2を使用しない場合は、COMを3V3に接続しておく事を推奨します。
J3/J4に接続された一部のGPIO端子だけがHATに配線されています。スペーサの長さ調整が必要ですが、エレベーテッド ソケットストリップ等を挿入する事でHATの表面からRaspberry PiのGPIOを全て引き出す事が可能です。
Attach file: SLG4E46882_DS_r010_08312023.pdf by takaboo
¶寸法 | 12x21mm |
絶対最大定格電圧 | -0.5~6.5V |
Attach file: SLG46108V(E150_DesignData).pdf by takaboo
TTL2DXIFは汎用のシリアルI/Fを2種類の半二重シリアルI/Fに変換するボードで、BTE094 TTL2DXIFの後継機です。半二重I/Fの入出力方向の切り替えは送信データと連動させても通信は可能ですが、ビットと連動して高速に入出力が切り替わる事の弊害もあるため、その場合は本ボードを介して切り替えの頻度を低減させ通信の安定性を図る事ができます。
Dynamixelシリーズがサポートする半二重TTLシリアル及びRS-485 I/Fとの互換性を持ち、また同種のI/Fで通信する他の装置に適用できます。
※本製品は半田付け作業を要求します。
※DXSHIELDに付属します。
型式 | 数量 | 備考 |
本体 | 1 | E150B |
ピンヘッダ | 1本 | 1x8ピン(2.54mmピッチ) |
コネクタ | 1個 | JST B3B-EH |
1個 | JST B4B-EH |
商品番号 | BTE094B | |
PCBリビジョン | E150B | |
動作温度範囲 | 0~70℃ 結露なきこと | |
寸法 | 12x21mm | |
重量 | ||
電源電圧範囲 | 3.0~5.0V | |
絶対最大定格電圧 | -0.5~6.5V | |
UART コンディション | RXD VIH min | 2.0V (VCC=3.3V) 3.5V (VCC=5.0V) |
RXD VIL max | 0.8V (VCC=3.3V) 1.5V (VCC=5.0V) | |
TXD VOH min | 3.3V (VCC=3.3V) 4.6V (VCC=5.0V) | |
TXD VOL max | 0.2V (VCC=3.3V) 0.3V (VCC=5.0V) | |
各I/F仕様 | TTL | 複信 半二重 最大通信速度 12Mbps 絶対最大定格 -0.5~6.5V ESD保護 MM=±2kV, HBM=200V |
RS-485 | 複信 半二重 最大通信速度 12Mbps 最大ノード数 256台 絶対最大定格 -70~70V ESD保護 CDM=±1.5kV, HBM=±16kV | |
環境配慮 | RoHS準拠, 鉛フリー半田 |
端子番号 | 信号名 |
1 | RS-485 D- (DM) |
2 | RS-485 D+ (DP) |
3 | TTL Signal (TTL) |
4 | GND |
端子番号 | 信号名 |
1 | GND |
2 | - |
3 | RS-485 D+ (DP) |
4 | RS-485 D- (DM) |
3ピンコネクタを採用したDynamixelをはじめ、その他のサーボモータは概ねこのI/Fを介して通信ができますが、I/Fのタイミングや電気的特性等の詳細が公開されていない製品への適用は注意が必要です。
マイコン等に備わっているUARTの送信端子とJ1-3(RXD)端子、受信端子とJ1-2(TXD)端子を相互に接続します。電源は3.3Vないし5Vに対応しますので、マイコンの動作電圧に合わせてJ1-1とJ1-4に適宜接続します。
J2-3(TTL Signal)端子はJ1-3(RXD)端子がLOWになると出力に、HIGHになると一定時間経過の後に入力に切り替わります。
J1への配線は極力短くし、J2への配線にはシールドケーブルの使用を推奨します。
4ピンコネクタを採用したDynamixelやその他のサーボモータは概ねこのI/Fで通信できますが、こちらもTTL I/F同様に詳細仕様が公開されていない装置への適用には注意が必要です。
マイコン等に備わっているUARTの送信端子とJ1-3(RXD)端子、受信端子とJ1-2(TXD)端子を相互に接続します。電源は3.3Vないし5Vに対応しますので、マイコンの動作電圧に合わせてJ1-1とJ1-4に適宜接続します。
J2-1/2(DM/DP)端子はJ1-3(RXD)端子がLOWになると出力に、HIGHになると一定時間経過の後に入力に切り替わります。
J1への配線は極力短くし、J2への配線にはツイストペアシールドケーブルの使用を推奨します。
デモに同梱したDXLIBはDynamixelのモデルを意識すること無くかつ物理値での指令に対応する追加APIが使用でき、それを利用して俗に言うモーションを簡単に再生するDXLMotionクラスを作った。
¶後に紹介する数十kHzの音データを出力する際は、わざわざ逓倍して出力する事になる。またキャリア周波数が高いため、実質的な分解能は落ちる。
なおarduino-pico Release 3.7.1以降に適用されたPWMキャリア周波数とDMA転送周期の分離を前提とした音再生を行っているので、それよりも古いライブラリではコンパイルできない。
Attach file: demo4.7z by takaboo
¶Attach file: demo3.7z by takaboo
¶Attach file: demo3.7z by takaboo
ここでは専ら音を鳴らす事を紹介。
item | qty |
Raspberry Pi Pico | 1 |
BTE110 PicoSHIELD | 1 |
BTE097 DX Busrepeater | 1 |
enclosure with speaker (3D pdf) | 1 |
Robot Cable-X4P 100mm | 1 |
Qwiic cable | 1 |
pin socket 2x6 | 1 |
頒布版はPicoはPicoSHIELDにピンソケットを介して装着し、スピーカを内蔵した電気的な絶縁を目的としたエンクロージャーに封入。
基板の短絡保護のためエンクロージャ(PLA+で生成)を作った上で2個のスピーカを内蔵し、PicoSHIELDのオーディオアンプに接続。基板に直接アクセスする際に分解する際は、ケーブル長が短いので断線させないよう少しずつこじ開ける事。
ちなみに少々まとめて出力したため、一部に変形がある。また嵌合力は積層同士の摩擦によるもので、開閉を繰り返したり熱を加えると弱まる。
PicoとPicoSHIELDの間はさほど長くないワイヤで接続してあるので、余程の事が無い限り分離しない事。
またPico WはArduino Picoライブラリにおける対応が不十分なところが見受けられるため、込み入った用途の場合は推奨しない。
PicoのUSBコネクタへ供給するUSBバスパワーでも運用できるが、オーディオへのノイズの流入が激しい。PicoSHIELDのCN2から安定した電源を給電する事を推奨する。
PicoSHIELDはDynamixel XシリーズのRS-485 I/F版を想定しているが、TTL I/F版を使用したい場合は同梱のDX BusrepeaterでI/Fを使って変換する。
通常はPicoに備わっているBOOTSELボタンで動作モードを切り替えるが、その場合は電源の操作が強いられる。PicoSHIELDではPB1の操作のみでPicoの動作モードを切り替える事ができる。
PB1を長押ししてPicoのブートローダをUSB接続のストレージ状態にし、コンパイル済みの*.uf2ファイルをそのストレージにコピーするだけなのだが、Arduino IDEを介すと転送に失敗するケースが散見される。
とりあえず転送に失敗するのと何かと面倒なため、こちらではArduino IDEではコンパイルだけを行い(メニューのスケッチ→コンパイル済みバイナリをエクスポート)、転送は個別に行うという手法が確立している。また転送する際はボリュームラベルを頼りにコピーするコマンドをあつらえた(Windows用のみ)。
コンパイル済みバイナリがエクスポートされたフォルダに展開し同梱の cp.cmd を起動すると、同じフォルダにある*.uf2ファイルをPicoのストレージを見つけた上でコピーしてくれる。
PicoのUSB(Serial)・UART0(Serial1)・UART1(Serial2)を使用して外部とのコミュニケーションを行う。起動時はUSBだが、何かしら受信したポート側に切り替わる。
pin | function |
CN1-2 | GP12(TXD0) |
CN1-3 | GP13(RXD0) |
CN1-5 | GND |
2chのPWMを活性化し、単純に高速でデータを転送しているだけ。PWM周波数が低いとアンプが発熱するという理由だけで可聴領域よりもはるかに高い192kHzのキャリア周波数を選んでいる。これより低い周波数でPWMを出力すると、平滑時のリプルにより無音であってもアンプが発熱、場合によってはエンクロージャーが熱により変形する。
なおarduino-pico Release 3.7.1以降に適用されたPWMキャリア周波数とDMA転送周期の分離を前提とした音再生を行っているので、それよりも古いライブラリではコンパイルできない。
様々なフォーマットを扱うのが面倒だったのでここではRAWデータのみを扱うが、データを保存する場所はメモリのサイズに依存する。
Picoのフラッシュはコードを含む2Mbyteが上限となる。
Picoのフラッシュに入り切らない程の音ネタを使うまでも無いと思うが、どうしてもと言う場合はmicroSDカードに保存して利用する事も可能。
音ネタのフォーマットをRAWファイルに変換するには、SoXを使うと簡単。とりあえずMP3からRAWデータに変換するバッチを含む実行ファイルを置いておく。
例えば44.1kHzステレオ符号付き16ビットRAWファイルに変換する場合は、同梱の conv_mp3_to_stereo44kraw.cmd に対して変換したいMP3ファイルをドラッグドロップする(複数も可)とconvertedフォルダにRAWファイルが生成される。
そもそもPicoをArduino IDEで使う事が間違いではあるが、いろいろなプラットホームに対応しているのでそれを使用する前提とする。もちろんPicoは単なるマイコンボードなのでプログラムなしに動く事はない。
間違いやすいのがArduino IDEにインストールするPico用のライブラリ。必ずこちらのリンクのライブラリを適用する事。
いずれのデモのアーカイブファイルのbuildフォルダ配下にコンパイル済みのバイナリと転送用のバッチが含まれているので、とりあえずArduino IDEが無くても動作確認は可能。
本来ならUSBカードリーダ化してmicroSDカードを挿抜せずともデータをPCから更新させるし、MP3等のフォーマットにも対応させるのだが、SDカードへのアクセスが致命的に遅いので止めている。MicroPythonを使ったとしても同様の制約を受けるのでイマイチ。
いずれにしても至極簡単かつ手抜きなツクリなので、適宜改造して活用してもらえればと。
本デモにおけるSerialの使い方の紹介。
3つのSerialのうち使用者がどれを選択するか分からないために、とりあえずどれか1つを自動的に選択するようにした。選択の条件は外部から送信がなされたポートかforceselで選択したポート。デフォルトはUSBでハードフロー制御を無効にするためにignoreFlowControl()を呼んでいる。
#include "SerialMulti.hpp" // インスタンス化 CSerialMulti SerialM; // 115.2kbpsで初期化 SerialM.begin(115200); // 書式化文字列送信 SerialM.printf("uhohon"); // 送信バッファフラッシュ SerialM.flush(); // 受信データ数取得 int n = SerialM.available(); // 受信データ読み出し char c = SerialM.read(); // ポートの強制選択 (0=Serial, 1=Serial1, 2=Serial2) SerialM.forcesel(2);
Picoに搭載されたフラッシュ上に配したRAWデータの再生。
フラッシュのサイズの都合もあるので、音データのフォーマットは無圧縮11.025kHzモノラル符号付き8ビットRAWデータとするが、再生時に周波数を変換できるので周波数はあまりこだわらない。フォーマットに従った音データを1.raw~9.rawの名前でファイルにし、コンパイル時にそれらのファイルを一緒にリンクして実行ファイルを生成する。
#include "RAWAudioPlayer.hpp" CRAWAudioPlayer RAWAudioPlayer; // 指定されたファイルをリンク INCBIN("hoge.raw", fuga); // リンクしたファイルを音データとして登録 std::vector<CRAWAudioPlayer::TPCMInfo> PCMInfo{ { (int8_t *)& fuga, (int)& _size_fuga, -1, 256, 127 }, }; // 初期化 RAWAudioPlayer.begin(& PCMInfo); // 0番目に登録したファイルをデフォルトの24(ド)の周波数で再生 RAWAudioPlayer.play(0, 24, 64);
異なる音源を合成して同時再生させる事ができるが、単純に足し算しているだけなので音割れに注意。またArduino IDEはソースをテンポラリにコピーした上でコンパイルするため、今回のような外部ファイルをリンクさせる際のファイル名として相対パスが使えない。
RAWファイルが保存されたmicroSDカードをPicoSHIELDのCN4に装着し再生。
音データのフォーマットは再生能力めいっぱいの無圧縮44.1kHzステレオ符号付き16ビットRAWデータ固定としている。そのフォーマットに従った音データを1.raw~9.rawの名前でファイルにし、予めmicroSDカードのルートに保存されている前提。
#include "SDAudioPlayer.hpp" CSDAudioPlayer SDAudioPlayer; // 初期化 SDAudioPlayer.begin(); // 指定されたファイルを再生 SDAudioPlayer.play("hoge.raw");
microSDカードはFAT32フォーマット(32GB以下)限定、使い倒したものは推奨せず、USBバスパワーではmicroSDカードへのアクセス時にアンプへ過度なノイズが加わるので外部電源推奨。
音つながりでPicoを昔ながらのサウンドチップ化しピコピコ音を鳴らす。
元ネタはDXMIOのサンプルで、PCMデータと合わせて簡易的なMMLを用いて楽曲を再生可。もともとシングルソースだったものを無理矢理分割したので少々強引。
#include "RAWAudioPlayer.hpp" #include "Synth.hpp" #include "TinyMusicSq.hpp" // インスタンス化 CRAWAudioPlayer RAWAudioPlayer; CTinyMusicSq MSq[_MAX_MUSICSQ]; // 初期化 RAWAudioPlayer.set_synthfunc(mixnote); // 波形設定 SetWaveForm(0, (const int8_t[]){ 0, -8, -16, -24, -32, -40, -48, -56, -64, -72, -80, -88, -96, -104, -112, -120, -128, 120, 112, 104, 96, 88, 80, 72, 64, 56, 48, 40, 32, 24, 16, -8 }, 32); // 再生 ConvertAndStartMusic(1, "t5@0o4l4v58$,50,100, [ cdefgab<cdefgab ]");
必要以上に無駄な事をしているのとサボり気味なので、鳴らなかったらご愛敬。
ロボット等の姿勢を検出するためにはIMUは必須だろうという事で、Qwiicに対応したadafruitのモジュールと通信するプログラム。
bno085monクラスはCEVA提供のライブラリを使ってBNO085と通信し、バックグラウンドでデータを取得し続けるためにFreeRTOSでタスクを生成している。
デモは取得を指示したデータの殆どをターミナルに吐き出す。また加速度・ジャイロ・地磁気のダイナミックキャリブレーションを有効にしてあるので、BNO08X Sensor Calibration Procedureを行う事でキャリブレーションが可能。
せっかくなのでDynamixelと通信するプログラムを作成。
デモに同梱したDXLIBはDynamixelのモデルを意識すること無くかつ物理値での指令に対応する追加APIが使用でき、それを利用して俗に言うモーションを簡単に再生するDXLMotionクラスを作った。
#include "DXLMotion.hpp" // インスタンス化 DXLMotion motion; // 対象IDの一覧 const std::vector<uint8_t> my_id_list {1,2,3}; // サーボの初期化 const DXLMotion::TServoInfo servo_info = { {1, 4, 0x4}, {2, 4, 0x4}, {3, 4, 0x4} }; motion.init_servo(& servo_info); // モーションデータの再生 const DXLMotion::TMotion motion1 = { my_id_list, { { { 0, 0, 0 }, 1.0 }, { { 45, 90, 30 }, 1.5 }, { { -30, 30, 0 }, 0.5 }, { { 123,-123, 270 }, 2.2 } } }; motion.play(& motion1);
デモは1MbpsでID=1~8のDynamixel X/Pシリーズを対象、モーションを再生する前に'i'を送信して接続されたDynamixelの検索と初期化が必須、Dynamixelは1台でもOK、モーションのフレーム切り替えにFreeRTOSを使用。
Pico W限定だが、ソケットとRS-485間をブリッジさせるプログラムで、PCと装置をUSBシリアル変換器等で接続している部分を無線化する。
基本的に単純なソケットサーバで、ポート番号23で待ち受け、ソケットで受信したデータをRS-485に送信、RS-485から受信したデータをソケットに送信するだけ。
APMODEのマクロでPico WそのものをWiFiのアクセスポイントにするか、Pico Wを既存のWiFiルータに接続するかを選択できる。詳細はソースにて。
COMポートを介して通信する既存のWindowsのアプリケーションを無線化する場合は、任意のIPアドレスとポート番号を割り当ててCOMポートを追加できるUSR-VCOMが使える。その場合はボーレート等の変更にも対応。
なおネットワーク環境やPCの負荷に大きく依存するため、既存のアプリの設定(特に受信タイムアウト)では正常に通信できない事も起こり得る。それを以てしても無線化する恩恵が大きい場合にのみ、本構成を検討する事。
def __init__(self, port : serial.Serial | str, baudrate = 57600, timeout = 0.05, mutex = None): if isinstance(port, serial.Serial): self.__serial = port self.__baudrate = port.baudrate self.__timeout = port.timeout else: print(port, baudrate, timeout) print(type(port)) self.__serial = serial.Serial(port, baudrate = baudrate, timeout = timeout) self.__baudrate = self.__serial.baudrate self.__timeout = self.__serial.timeout if mutex == None: self.__mutex = threading.Lock() else: self.__mutex = mutex return self.__serial.baudrate def __reconfig(self): self.__serial.baudrate = self.__baudrate self.__serial.timeout = self.__timeout
self.__reconfig() def FullScan(self) -> tuple: orgtimeout = self.__timeout orgbaudrate = self.__baudrate #baud=(3000000,2000000,1500000,1250000,1000000,625000,115200,57600) baud=(57600,115200,625000,1000000,1250000,1500000,2000000,3000000) self.__serial.timeout = 0.001 for b in baud: for id in range(240): self.__serial.baudrate = b r = self.MemREAD8(id, 0) print(f'baud:{b:7} id:{id:3} :', end='findn' if r else 'noner') self.__serial.timeout = orgtimeout self.__baudrate = orgbaudrate
try: pmx = PMXProtocol('\\.\COM12', 115200) except: pass else: ID = 0 # reboot print('ReBoot=', pmx.ReBoot(ID, echo=True)) time.sleep(0.5) ''' # factory reset print('FactoryReset=',pmx.FactoryReset(ID)) time.sleep(0.5) ''' # load print('LOAD=', pmx.LOAD(ID, echo=True)) # save print('SAVE=', pmx.SAVE(ID, echo=True)) ''' # sys write print('SysWrite=',pmx.SystemWRITE(ID, (ID, pmx.SYSW_BAUD_115_2k, pmx.SYSW_PARITY_NONE, 20))) time.sleep(0.5) ''' # sys read r = pmx.SystemREAD(ID) print('SysRead=', f'${r[0]:08x} ${r[1]:08x} ${r[2]:08x} {r[3]:d} ' if r else 'False') # dump memory print('dump') for addr in range(0, 800, 20): r = pmx.MemREAD(ID, addr, 20) print(f' MemREAD({addr}:{20})=', r.hex(':') if r else '!!!!!!!!!!!!!!!!!!!!!!!!') # gain r = pmx.MemREAD(ID, 0, 64) print('gainn MemREAD(0:64)=', *iter_unpack('<IIIIIIIIIIIIIIII', r) if r else '!!!!!!!!') # voltage limit r = pmx.MemREAD(ID, 76, 8) print('volt limn MemREAD(76:8)=', *iter_unpack('<HHHH', r) if r else '!!!!!!!!') # present value r = pmx.MemREAD(ID, 300, 24) print('present valn MemREAD(300:20)=', *iter_unpack('<hhhhhhhhhHHH', r) if r else '!!!!!!!!') # error stat r = pmx.MemREAD(ID, 400, 6) print('err statn MemREAD(0:400:7)=', *iter_unpack('<BBBxH', r) if r else '!!!!!!!!') print('preparation for pos ctrl') # ctrl off print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 1)) print(' MemWRITE8(502)=', pmx.MemWRITE8(ID, 502, 0b11111)) time.sleep(0.1) # goal angle print('start pos ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for ang in tuple(range(0, 32000, 500)) + tuple(range(32000, -32000, -500)) + tuple(range(-32000, 500, 500)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (ang,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) continue pass break print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ()))
print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 2)) time.sleep(0.1)
# goal angle print('start speed ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for spd in tuple(range(0, 2000, 50)) + tuple(range(2000, -2000, -50)) + tuple(range(-2000, 50, 50)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (spd,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) else: continue pass break else: print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ()))
del pmx
r = self.MemREAD(id, addr, 2 * length, echo) r = self.MemREAD(id, addr, 4 * length, echo)
r = self.MemREAD(id, addr, 2 * length, echo) r = self.MemREAD(id, addr, 4 * length, echo)
PicoSHIELD+Raspberry Pi Picoを対象にPythonのコードを修正した。
¶PicoSHIELD+Raspberry Pi Picoを対象に修正してみた。
# pyPMX.py for MicroPython(RP2040) # (C) 2024 T.Uemitsu from machine import UART, Pin import time, array from collections import namedtuple from struct import pack, unpack ########################################################## # Functionalized the part of converting int to bytes. # If specified as a tuple, it is converted to bytes at once. ########################################################## @micropython.native def B2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return bytes(((d & 0x7f) | 0x80) if d < 0 else d for d in d) else: return bytes(pack('<B', ((d & 0x7f) | 0x80) if d < 0 else d)) @micropython.native def W2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<H',d) for d in [((d & 0x7fff) | 0x8000) if d < 0 else d for d in d]]) else: return bytes(pack('<H', ((d & 0x7fff) | 0x8000) if d < 0 else d)) @micropython.native def L2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<I',d) for d in [((d & 0x7fffffff) | 0x80000000) if d < 0 else d for d in d]]) else: return bytes(pack('<I', ((d & 0x7fffffff) | 0x80000000) if d < 0 else d)) ########################################################## # API for Kondo PMX ########################################################## class PMXProtocol: BROADCASTING_ID = const(0xff) CMD_MemREAD = const(0xa0) CMD_MemWRITE = const(0xa1) CMD_LOAD = const(0xa2) CMD_SAVE = const(0xa3) CMD_MotorREAD = const(0xa4) CMD_MotorWRITE = const(0xa5) CMD_SystemREAD = const(0xbb) CMD_SystemWRITE = const(0xbc) CMD_ReBoot = const(0xbd) CMD_FactoryReset = const(0xbe) CMD_SystemINIT = const(0xbf) (SYSW_BAUD_57600, SYSW_BAUD_115_2k, SYSW_BAUD_625k, SYSW_BAUD_1M, SYSW_BAUD_1_25M, SYSW_BAUD_1_5M, SYSW_BAUD_2M, SYSW_BAUD_3M) = range(8) (SYSW_PARITY_NONE, SYSW_PARITY_ODD, SYSW_PARITY_EVEN) = range(3) (MOTW_OPT_NONE, MOTW_OPT_TORQUEON, MOTW_OPT_FREE, _, MOTW_OPT_BRAKE, _, _, _, MOTW_OPT_HOLD) = range(9) __crc16_lutable = array.array('H',( 0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef, 0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de, 0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d, 0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc, 0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b, 0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a, 0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49, 0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78, 0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067, 0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256, 0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405, 0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634, 0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3, 0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92, 0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x01ce0,0xcc1, 0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0 )) def __init__(self, baudrate = 115200, timeout_ms = 50): """ Initalize parameters ------------- baudrate : int Serial baudrate[bps] timeout_ms : float Read timeout[s] """ self.__Error = 0 self.__baudrate = baudrate self.__timeout = timeout_ms self.__status = 0 self.__serial = UART(1, baudrate, tx = Pin(4), rx = Pin(5), txbuf = 500, rxbuf = 500, timeout = timeout_ms) @property def baudrate(self): return self.__baudrate @baudrate.setter def baudrate(self, baudrate): self.__baudrate = baudrate self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def timeout(self): return self.__timeout @timeout.setter def timeout(self, timeout_ms): self.__timeout = timeout_ms self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def status(self): return self.__status @micropython.viper def __crc16(self, dat : ptr8, l:int) -> int: crc:int = 0 pt = ptr16(self.__crc16_lutable) for i in range(l): crc = (crc << 8) ^ pt[((crc >> 8) ^ dat[i]) & 0xff] return crc & 0xffff @micropython.native def TxPacket(self, id : int, cmd : int, opt : int, param : bytes, echo = False) -> (bytes, bool): if ((id >= 0 and id <= 239) or id == BROADCASTING_ID) and len(param) <= (256 - 8): txp = bytes([0xfe,0xfe,id,len(param) + 8,cmd,opt]) + bytes(param) txp += W2Bs(self.__crc16(txp, len(txp))) _ = self.__serial.read(self.__serial.any()) if echo: print('TX:', txp.hex(':')) self.__serial.write(txp) return txp, True return None, False @micropython.native def __rx(self, length) -> bytes: s = self.__serial.read(length) if s: l = len(s) if l == length: return s else: r = s length -= l while self.__serial.any() > 0: s = self.__serial.read(length) r += s length -= len(s) if length == 0: break return r else: return bytes() @micropython.native def RxPacket(self, echo = False) -> (bytes, bool): self.__serial.flush() rxp = self.__rx(6) if rxp: if len(rxp) == 6: l = rxp[3] - 6 rxp += self.__rx(l) if len(rxp) == l + 6 and rxp[0] == 0xfe and rxp[1] == 0xfe and unpack('<H', rxp[-2:])[0] == self.__crc16(rxp[:-2], len(rxp[:-2])): self.__status = rxp[5] if echo: print('RX:', rxp.hex(':')) return bytes(rxp), True return None, False @micropython.native def MemWRITE(self, id : int, addr : int, data : bytes, echo = False) -> bool: if ((id >= 0 and id <= 239) or id == BROADCASTING_ID) and addr >= 0 and addr <= 0x4ff: if self.TxPacket(id, CMD_MemWRITE, 1, W2Bs(addr) + data, echo)[1]: if id != BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return (d[2] == id) and (d[4] == 0x21) else: return True return False @micropython.native def MemWRITE8(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, B2Bs(data), echo) @micropython.native def MemWRITE16(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, W2Bs(data), echo) @micropython.native def MemWRITE32(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, L2Bs(data), echo) @micropython.native def MemREAD(self, id : int, addr : int, length : int, echo = False) -> bytes: if addr >= 0 and id <= 239 and addr >= 0 and addr <= 0x4ff and length > 0 and length > 0 and length <= 247: if self.TxPacket(id, CMD_MemREAD, 0, W2Bs(addr) + B2Bs(length), echo)[1]: d, r = self.RxPacket(echo) if r: if d[4] == 0x20 and id == d[2] and (d[5] & (4 + 8 + 0x10 + 0x40)) == 0: return bytes(d[6:-2]) return None @micropython.native def MemREAD8(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, length, echo) if r != None: n = sum(iter_unpack('b' if signed else 'B', r), ()) return n if length > 1 else n[0] return None @micropython.native def MemREAD16(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, 2 << (length - 1), echo) if r != None: n = sum(iter_unpack('h' if signed else 'H', r), ()) return n if length > 1 else n[0] return None @micropython.native def MemREAD32(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, 4 << (length - 1), echo) if r != None: n = sum(iter_unpack('i' if signed else 'I', r), ()) return n if length > 1 else n[0] return None @micropython.native def LOAD(self, id : int, echo = False) -> bool: if self.TxPacket(id, CMD_LOAD, 0, (), echo)[1]: if id != BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x22 else: return True; return False @micropython.native def SAVE(self, id : int, echo = False) -> bool: if self.TxPacket(id, CMD_SAVE, 0, (), echo)[1]: if id != BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x23 else: return True; return False @micropython.native def MotorREAD(self, id : int, echo = False) -> tuple: if self.TxPacket(id, CMD_MotorREAD, 0, (), echo): if id != BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return tuple([dat[6], tuple(n[0] for n in iter_unpack('<h', dat[7:-2])) if len(dat[7:-2]) > 0 and dat[4] == 0x24 else ()]) else: return None else: return () return None @micropython.native def MotorWRITE(self, id : int, opt : int, dat : (), echo = False) -> tuple: if self.TxPacket(id, CMD_MotorWRITE, opt, W2Bs(dat), echo): if id != BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return tuple([dat[6], unpack('<'+'h'*(len(dat[7:-2])>>1), dat[7:-2]) if len(dat[7:-2]) > 0 and dat[4] == 0x25 else ()]) else: return None else: return () return None @micropython.native def SystemREAD(self, id : int, echo = False) -> tuple: if self.TxPacket(id, CMD_SystemREAD, 0, (), echo)[1]: d, r = self.RxPacket(echo) if r: if len(d[6:-2]) == 13 and d[4] == 0x3b: return unpack('<IIIB', d[6:-2]) return None @micropython.native def SystemWRITE(self, id : int, data : (), echo = False) -> bool: if data[0] >= 0 and data[0] <= 239 and data[1] >= 0 and data[1] <= 7 and data[2] >= 0 and data[2] <= 2 and data[3] >= 1 and data[3] <= 255: d = self.SystemREAD(id, echo) if d: with self.__mutex: if self.TxPacket(id, CMD_SystemWRITE, 0xf, L2Bs(d[0]) + B2Bs(data), echo)[1]: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x3c and (d[5] & (4 + 8 + 0x10 + 0x40)) == 0 return False @micropython.native def ReBoot(self, id : int, echo = False) -> bool: if id != BROADCASTING_ID: if self.TxPacket(id, CMD_ReBoot, 0, W2Bs(0), echo)[1]: d, r = self.RxPacket(echo) if r: return d[2] == id and d[4] == 0x3d return False @micropython.native def FactoryReset(self, id : int, echo = False) -> bool: if id != BROADCASTING_ID: d = self.SystemREAD(id, echo) if d != None: if self.TxPacket(id, CMD_FactoryReset, 0, L2Bs(d[0]), echo)[1]: d, r = self.RxPacket(echo) if r: return d[2] == id and d[4] == 0x3e return False @micropython.native def FullScan(self) -> tuple: orgtimeout = self.__timeout orgbaudrate = self.__baudrate #baud=(3000000,2000000,1500000,1250000,1000000,625000,115200,57600) baud=(57600,115200,625000,1000000,1250000,1500000,2000000,3000000) self.__serial.timeout = 0.001 for b in baud: for id in range(240): self.__serial.baudrate = b r = self.MemREAD8(id, 0) print(f'baud:{b:7} id:{id:3} :', end='findn' if r else 'noner') self.__serial.timeout = orgtimeout self.__baudrate = orgbaudrate ########################################################## # test code ########################################################## if __name__ == "__main__": import time from pyPMX import * machine.freq(250000000) # set the CPU frequency to 250 MHz try: pmx = PMXProtocol() except: pass else: ID = 0 # reboot print('ReBoot=', pmx.ReBoot(ID, echo=True)) time.sleep(0.5) ''' # factory reset print('FactoryReset=',pmx.FactoryReset(ID)) time.sleep(0.5) ''' # load print('LOAD=', pmx.LOAD(ID, echo=True)) # save print('SAVE=', pmx.SAVE(ID, echo=True)) ''' # sys write print('SysWrite=',pmx.SystemWRITE(ID, (ID, pmx.SYSW_BAUD_115_2k, pmx.SYSW_PARITY_NONE, 20))) time.sleep(0.5) ''' # sys read r = pmx.SystemREAD(ID, echo=True) print('SysRead=', f'${r[0]:08x} ${r[1]:08x} ${r[2]:08x} {r[3]:d} ' if r else 'False') # dump memory print('dump') for addr in range(0, 800, 20): r = pmx.MemREAD(ID, addr, 20) print(f' MemREAD({addr}:{20})=', r.hex(':') if r else '!!!!!!!!!!!!!!!!!!!!!!!!') # gain r = pmx.MemREAD(ID, 0, 64) print('gainn MemREAD(0:64)=', unpack('<IIIIIIIIIIIIIIII', r) if r else '!!!!!!!!') # voltage limit r = pmx.MemREAD(ID, 76, 8) print('volt limn MemREAD(76:8)=', unpack('<HHHH', r) if r else '!!!!!!!!') # present value r = pmx.MemREAD(ID, 300, 24) print('present valn MemREAD(300:20)=', unpack('<hhhhhhhhhHHH', r) if r else '!!!!!!!!') # error stat r = pmx.MemREAD(ID, 400, 6) print('err statn MemREAD(0:400:7)=', unpack('<BBBxH', r) if r else '!!!!!!!!') print('preparation for pos ctrl') # ctrl off print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 1)) print(' MemWRITE8(502)=', pmx.MemWRITE8(ID, 502, 0b11111)) time.sleep(0.1) # goal angle print('start pos ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for ang in tuple(range(0, 32000, 500)) + tuple(range(32000, -32000, -500)) + tuple(range(-32000, 500, 500)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (ang,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) else: continue pass break else: print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 2)) time.sleep(0.1) # goal angle print('start speed ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for spd in tuple(range(0, 2000, 50)) + tuple(range(2000, -2000, -50)) + tuple(range(-2000, 50, 50)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (spd,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) else: continue pass break else: print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) del pmx print('fin')
動かすだけならここだけおさえておけば良しという事で、公式サイトのリンクをブックマーク。
PMXシリーズ
PMXサーボシリーズオンラインマニュアル
B3Mシリーズと大差ないと思いきや、コネクタが全く違う。さらに標準のケーブルがぶっとい。DXHUBにつなぎたかったので、コレを使ってケーブルを自作。
ひとまず電源は手持ちのACアダプタを使う事にしたが、結果的に市販の物では全く容量足りず。トラブルを避けるには、ガッツリ電流取れる電池か安定化電源用意した方が良いだろう。
聞くところによるとファームウェアは日々更新されているらしく、必ず新しいバージョンを適用して使うべしとの事。
確かにそのままでは後に紹介するスクリプトがエラーになる事があったので、有無も言わさず更新すべきかと。
上澄みだけさらった感触を書いてみる。
DXLやB3Mシリーズを使ったことがあれば運用は簡単。PICのイメージが色濃いKRSとは一線を画すので、それに慣れ切っていると難しい。
B3Mからの仕切り直し感を感じるので、種々のリクエストを吸収してもらえる事を大いに期待し、初期リリース版に触れて感じた点も書いておく。
ここら辺にある事は既知だが、それをあえて見ずに基本的なコマンドを関数にしたクラスを作った。
ホントはDXL用のスクリプトを作ったついでにあつらえたのだが、メモリマップ上に配置された情報を読み書きするという概念は同じなので、あっさりと実現。
なおbytesを直接的に扱うのは避けたかったので、一部を除きできるだけint型とリストやタプルで記述できるようにした。使い方は末尾のデモコードを参考にすれば大半は理解できるかと思う。
#!/usr/bin/python3 # -*- coding: utf-8 -*- # # pyPMX.py # (C) 2024 T.Uemitsu import serial, threading, array, struct from collections import namedtuple from struct import pack, unpack, iter_unpack ########################################################## # Functionalized the part of converting int to bytes. # If specified as a tuple, it is converted to bytes at once. ########################################################## def B2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return bytes(((d & 0x7f) | 0x80) if d < 0 else d for d in d) else: return bytes(pack('<B', ((d & 0x7f) | 0x80) if d < 0 else d)) def W2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<H',d) for d in [((d & 0x7fff) | 0x8000) if d < 0 else d for d in d]]) else: return bytes(pack('<H', ((d & 0x7fff) | 0x8000) if d < 0 else d)) def L2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<I',d) for d in [((d & 0x7fffffff) | 0x80000000) if d < 0 else d for d in d]]) else: return bytes(pack('<I', ((d & 0x7fffffff) | 0x80000000) if d < 0 else d)) ########################################################## # API for Kondo PMX ########################################################## class PMXProtocol: BROADCASTING_ID = 0xff CMD_MemREAD = 0xa0 CMD_MemWRITE = 0xa1 CMD_LOAD = 0xa2 CMD_SAVE = 0xa3 CMD_MotorREAD = 0xa4 CMD_MotorWRITE = 0xa5 CMD_SystemREAD = 0xbb CMD_SystemWRITE = 0xbc CMD_ReBoot = 0xbd CMD_FactoryReset = 0xbe CMD_SystemINIT = 0xbf (SYSW_BAUD_57600, SYSW_BAUD_115_2k, SYSW_BAUD_625k, SYSW_BAUD_1M, SYSW_BAUD_1_25M, SYSW_BAUD_1_5M, SYSW_BAUD_2M, SYSW_BAUD_3M) = range(8) (SYSW_PARITY_NONE, SYSW_PARITY_ODD, SYSW_PARITY_EVEN) = range(3) (MOTW_OPT_NONE, MOTW_OPT_TORQUEON, MOTW_OPT_FREE, _, MOTW_OPT_BRAKE, _, _, _, MOTW_OPT_HOLD) = range(9) __crc16_lutable = array.array('H') def __init__(self, port : serial.Serial | str, baudrate = 57600, timeout = 0.05, mutex = None): if isinstance(port, serial.Serial): self.__serial = port self.__baudrate = port.baudrate self.__timeout = port.timeout else: print(port, baudrate, timeout) print(type(port)) self.__serial = serial.Serial(port, baudrate = baudrate, timeout = timeout) self.__baudrate = self.__serial.baudrate self.__timeout = self.__serial.timeout if mutex == None: self.__mutex = threading.Lock() else: self.__mutex = mutex self.__status = 0 poly = 0x1021 for i in range(256): nAccum = i << 8 for j in range(8): nAccum = (poly ^ (nAccum << 1) if nAccum & 0x8000 else nAccum << 1) & 0xffff self.__crc16_lutable.append(nAccum) def __del__(self): del self.__crc16_lutable[:] @property def mutex(self): return self.__mutex @property def baudrate(self): return self.__serial.baudrate @baudrate.setter def baudrate(self, baudrate): self.__baudrate = baudrate self.__serial.baudrate = baudrate @property def timeout(self): return self.__serial.timeout @timeout.setter def timeout(self, timeout): self.__timeout = timeout self.__serial.timeout = timeout def __reconfig(self): self.__serial.baudrate = self.__baudrate self.__serial.timeout = self.__timeout @property def status(self): return self.__status def __crc16(self, data : bytes) -> int: crc = 0 for d in data: crc = (crc << 8) ^ self.__crc16_lutable[((crc >> 8) ^ d) & 0xff] return crc & 0xffff def TxPacket(self, id : int, cmd : int, opt : int, param : bytes, echo = False) -> (bytes, bool): self.__reconfig() if ((id >= 0 and id <= 239) or id == self.BROADCASTING_ID) and len(param) <= (256 - 8): txp = bytes([0xfe,0xfe,id,len(param) + 8,cmd,opt]) + bytes(param) txp += W2Bs(self.__crc16(txp)) self.__serial.reset_input_buffer() if echo: print('TX:', txp.hex(':')) self.__serial.write(txp) return txp, True return None, False def __rx(self, length) -> bytes: s = self.__serial.read(length) l = len(s) if l == length: return s else: r = s length -= l while self.__serial.in_waiting > 0: s = self.__serial.read(length) r += s length -= len(s) if length == 0: break return r def RxPacket(self, echo = False) -> (bytes, bool): self.__serial.flush() rxp = self.__rx(6) if rxp: if len(rxp) == 6: l = rxp[3] - 6 rxp += self.__rx(l) if len(rxp) == l + 6 and rxp[0] == 0xfe and rxp[1] == 0xfe and unpack('<H', rxp[-2:])[0] == self.__crc16(rxp[:-2]): self.__status = rxp[5] if echo: print('RX:', rxp.hex(':')) return bytes(rxp), True return None, False def MemWRITE(self, id : int, addr : int, data : bytes, echo = False) -> bool: with self.__mutex: if ((id >= 0 and id <= 239) or id == self.BROADCASTING_ID) and addr >= 0 and addr <= 0x4ff: if self.TxPacket(id, self.CMD_MemWRITE, 1, W2Bs(addr) + data, echo)[1]: if id != self.BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return (d[2] == id) and (d[4] == 0x21) else: return True return False def MemWRITE8(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, B2Bs(data), echo) def MemWRITE16(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, W2Bs(data), echo) def MemWRITE32(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, L2Bs(data), echo) def MemREAD(self, id : int, addr : int, length : int, echo = False) -> bytes: with self.__mutex: if addr >= 0 and id <= 239 and addr >= 0 and addr <= 0x4ff and length > 0 and length > 0 and length <= 247: if self.TxPacket(id, self.CMD_MemREAD, 0, W2Bs(addr) + B2Bs(length), echo)[1]: d, r = self.RxPacket(echo) if r: if d[4] == 0x20 and id == d[2] and (d[5] & (4 + 8 + 0x10 + 0x40)) == 0: return bytes(d[6:-2]) return None def MemREAD8(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, length, echo) if r != None: n = sum(iter_unpack('b' if signed else 'B', r), ()) return n if length > 1 else n[0] return None def MemREAD16(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, 2 * length, echo) if r != None: n = sum(iter_unpack('h' if signed else 'H', r), ()) return n if length > 1 else n[0] return None def MemREAD32(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, 4 * length, echo) if r != None: n = sum(iter_unpack('i' if signed else 'I', r), ()) return n if length > 1 else n[0] return None def LOAD(self, id : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.CMD_LOAD, 0, (), echo)[1]: if id != self.BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x22 else: return True; return False def SAVE(self, id : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.CMD_SAVE, 0, (), echo)[1]: if id != self.BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x23 else: return True; return False def MotorREAD(self, id : int, echo = False) -> tuple: with self.__mutex: if self.TxPacket(id, self.CMD_MotorREAD, 0, (), echo): if id != self.BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return tuple([dat[6], tuple(n[0] for n in iter_unpack('<h', dat[7:-2])) if len(dat[7:-2]) > 0 and dat[4] == 0x24 else ()]) else: return None else: return () return None def MotorWRITE(self, id : int, opt : int, dat : (), echo = False) -> tuple: with self.__mutex: if self.TxPacket(id, self.CMD_MotorWRITE, opt, W2Bs(dat), echo): if id != self.BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return tuple([dat[6], tuple(n[0] for n in iter_unpack('<h', dat[7:-2])) if len(dat[7:-2]) > 0 and dat[4] == 0x25 else ()]) else: return None else: return () return None def SystemREAD(self, id : int, echo = False) -> tuple: with self.__mutex: if self.TxPacket(id, self.CMD_SystemREAD, 0, (), echo)[1]: d, r = self.RxPacket(echo) if r: if len(d[6:-2]) == 13 and d[4] == 0x3b: v = tuple(iter_unpack('<IIIB', d[6:-2]))[0] return tuple(v) return None def SystemWRITE(self, id : int, data : (), echo = False) -> bool: if data[0] >= 0 and data[0] <= 239 and data[1] >= 0 and data[1] <= 7 and data[2] >= 0 and data[2] <= 2 and data[3] >= 1 and data[3] <= 255: d = self.SystemREAD(id, echo) if d: with self.__mutex: if self.TxPacket(id, self.CMD_SystemWRITE, 0xf, L2Bs(d[0]) + B2Bs(data), echo)[1]: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x3c and (d[5] & (4 + 8 + 0x10 + 0x40)) == 0 return False def ReBoot(self, id : int, echo = False) -> bool: with self.__mutex: if id != self.BROADCASTING_ID: if self.TxPacket(id, self.CMD_ReBoot, 0, W2Bs(0), echo)[1]: d, r = self.RxPacket(echo) if r: return d[2] == id and d[4] == 0x3d return False def FactoryReset(self, id : int, echo = False) -> bool: if id != self.BROADCASTING_ID: d = self.SystemREAD(id, echo) if d != None: with self.__mutex: if self.TxPacket(id, self.CMD_FactoryReset, 0, L2Bs(d[0]), echo)[1]: d, r = self.RxPacket(echo) if r: return d[2] == id and d[4] == 0x3e return False def FullScan(self) -> tuple: orgtimeout = self.__timeout orgbaudrate = self.__baudrate #baud=(3000000,2000000,1500000,1250000,1000000,625000,115200,57600) baud=(57600,115200,625000,1000000,1250000,1500000,2000000,3000000) self.__serial.timeout = 0.001 for b in baud: for id in range(240): self.__serial.baudrate = b r = self.MemREAD8(id, 0) print(f'baud:{b:7} id:{id:3} :', end='findn' if r else 'noner') self.__serial.timeout = orgtimeout self.__baudrate = orgbaudrate ########################################################## # test code ########################################################## if __name__ == "__main__": import time from pyPMX import * try: pmx = PMXProtocol('\\.\COM12', 115200) except: pass else: ID = 0 # reboot print('ReBoot=', pmx.ReBoot(ID, echo=True)) time.sleep(0.5) ''' # factory reset print('FactoryReset=',pmx.FactoryReset(ID)) time.sleep(0.5) ''' # load print('LOAD=', pmx.LOAD(ID, echo=True)) # save print('SAVE=', pmx.SAVE(ID, echo=True)) ''' # sys write print('SysWrite=',pmx.SystemWRITE(ID, (ID, pmx.SYSW_BAUD_115_2k, pmx.SYSW_PARITY_NONE, 20))) time.sleep(0.5) ''' # sys read r = pmx.SystemREAD(ID) print('SysRead=', f'${r[0]:08x} ${r[1]:08x} ${r[2]:08x} {r[3]:d} ' if r else 'False') # dump memory print('dump') for addr in range(0, 800, 20): r = pmx.MemREAD(ID, addr, 20) print(f' MemREAD({addr}:{20})=', r.hex(':') if r else '!!!!!!!!!!!!!!!!!!!!!!!!') # gain r = pmx.MemREAD(ID, 0, 64) print('gainn MemREAD(0:64)=', *iter_unpack('<IIIIIIIIIIIIIIII', r) if r else '!!!!!!!!') # voltage limit r = pmx.MemREAD(ID, 76, 8) print('volt limn MemREAD(76:8)=', *iter_unpack('<HHHH', r) if r else '!!!!!!!!') # present value r = pmx.MemREAD(ID, 300, 24) print('present valn MemREAD(300:20)=', *iter_unpack('<hhhhhhhhhHHH', r) if r else '!!!!!!!!') # error stat r = pmx.MemREAD(ID, 400, 6) print('err statn MemREAD(0:400:7)=', *iter_unpack('<BBBxH', r) if r else '!!!!!!!!') print('preparation for pos ctrl') # ctrl off print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 1)) print(' MemWRITE8(502)=', pmx.MemWRITE8(ID, 502, 0b11111)) time.sleep(0.1) # goal angle print('start pos ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for ang in tuple(range(0, 32000, 500)) + tuple(range(32000, -32000, -500)) + tuple(range(-32000, 500, 500)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (ang,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) else: continue pass break else: print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 2)) time.sleep(0.1) # goal angle print('start speed ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for spd in tuple(range(0, 2000, 50)) + tuple(range(2000, -2000, -50)) + tuple(range(-2000, 50, 50)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (spd,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) else: continue pass break else: print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) del pmx print('fin')
こちらでは主にここにあるWindows環境の未リリース版イメージで試しているが、基本的にはMSYS2をインストールしておけばその中で動かせる。もちろんLinuxやmacでも動くので、気楽に試してみてはどうだろう。
PicoSHIELD+Raspberry Pi Picoを対象にPythonのコードを修正した。
# pyPMX.py for MicroPython(RP2040) # (C) 2024 T.Uemitsu from machine import UART, Pin import time, array from collections import namedtuple from struct import pack, unpack ########################################################## # Functionalized the part of converting int to bytes. # If specified as a tuple, it is converted to bytes at once. ########################################################## @micropython.native def B2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return bytes(((d & 0x7f) | 0x80) if d < 0 else d for d in d) else: return bytes(pack('<B', ((d & 0x7f) | 0x80) if d < 0 else d)) @micropython.native def W2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<H',d) for d in [((d & 0x7fff) | 0x8000) if d < 0 else d for d in d]]) else: return bytes(pack('<H', ((d & 0x7fff) | 0x8000) if d < 0 else d)) @micropython.native def L2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<I',d) for d in [((d & 0x7fffffff) | 0x80000000) if d < 0 else d for d in d]]) else: return bytes(pack('<I', ((d & 0x7fffffff) | 0x80000000) if d < 0 else d)) ########################################################## # API for Kondo PMX ########################################################## class PMXProtocol: BROADCASTING_ID = const(0xff) CMD_MemREAD = const(0xa0) CMD_MemWRITE = const(0xa1) CMD_LOAD = const(0xa2) CMD_SAVE = const(0xa3) CMD_MotorREAD = const(0xa4) CMD_MotorWRITE = const(0xa5) CMD_SystemREAD = const(0xbb) CMD_SystemWRITE = const(0xbc) CMD_ReBoot = const(0xbd) CMD_FactoryReset = const(0xbe) CMD_SystemINIT = const(0xbf) (SYSW_BAUD_57600, SYSW_BAUD_115_2k, SYSW_BAUD_625k, SYSW_BAUD_1M, SYSW_BAUD_1_25M, SYSW_BAUD_1_5M, SYSW_BAUD_2M, SYSW_BAUD_3M) = range(8) (SYSW_PARITY_NONE, SYSW_PARITY_ODD, SYSW_PARITY_EVEN) = range(3) (MOTW_OPT_NONE, MOTW_OPT_TORQUEON, MOTW_OPT_FREE, _, MOTW_OPT_BRAKE, _, _, _, MOTW_OPT_HOLD) = range(9) __crc16_lutable = array.array('H',( 0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef, 0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de, 0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d, 0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc, 0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b, 0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a, 0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49, 0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78, 0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067, 0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256, 0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405, 0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634, 0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3, 0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92, 0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x01ce0,0xcc1, 0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0 )) def __init__(self, baudrate = 115200, timeout_ms = 50): """ Initalize parameters ------------- baudrate : int Serial baudrate[bps] timeout_ms : float Read timeout[s] """ self.__Error = 0 self.__baudrate = baudrate self.__timeout = timeout_ms self.__status = 0 self.__serial = UART(1, baudrate, tx = Pin(4), rx = Pin(5), txbuf = 500, rxbuf = 500, timeout = timeout_ms) @property def baudrate(self): return self.__baudrate @baudrate.setter def baudrate(self, baudrate): self.__baudrate = baudrate self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def timeout(self): return self.__timeout @timeout.setter def timeout(self, timeout_ms): self.__timeout = timeout_ms self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def status(self): return self.__status @micropython.viper def __crc16(self, dat : ptr8, l:int) -> int: crc:int = 0 pt = ptr16(self.__crc16_lutable) for i in range(l): crc = (crc << 8) ^ pt[((crc >> 8) ^ dat[i]) & 0xff] return crc & 0xffff @micropython.native def TxPacket(self, id : int, cmd : int, opt : int, param : bytes, echo = False) -> (bytes, bool): if ((id >= 0 and id <= 239) or id == BROADCASTING_ID) and len(param) <= (256 - 8): txp = bytes([0xfe,0xfe,id,len(param) + 8,cmd,opt]) + bytes(param) txp += W2Bs(self.__crc16(txp, len(txp))) _ = self.__serial.read(self.__serial.any()) if echo: print('TX:', txp.hex(':')) self.__serial.write(txp) return txp, True return None, False @micropython.native def __rx(self, length) -> bytes: s = self.__serial.read(length) if s: l = len(s) if l == length: return s else: r = s length -= l while self.__serial.any() > 0: s = self.__serial.read(length) r += s length -= len(s) if length == 0: break return r else: return bytes() @micropython.native def RxPacket(self, echo = False) -> (bytes, bool): self.__serial.flush() rxp = self.__rx(6) if rxp: if len(rxp) == 6: l = rxp[3] - 6 rxp += self.__rx(l) if len(rxp) == l + 6 and rxp[0] == 0xfe and rxp[1] == 0xfe and unpack('<H', rxp[-2:])[0] == self.__crc16(rxp[:-2], len(rxp[:-2])): self.__status = rxp[5] if echo: print('RX:', rxp.hex(':')) return bytes(rxp), True return None, False @micropython.native def MemWRITE(self, id : int, addr : int, data : bytes, echo = False) -> bool: if ((id >= 0 and id <= 239) or id == BROADCASTING_ID) and addr >= 0 and addr <= 0x4ff: if self.TxPacket(id, CMD_MemWRITE, 1, W2Bs(addr) + data, echo)[1]: if id != BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return (d[2] == id) and (d[4] == 0x21) else: return True return False @micropython.native def MemWRITE8(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, B2Bs(data), echo) @micropython.native def MemWRITE16(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, W2Bs(data), echo) @micropython.native def MemWRITE32(self, id : int, addr : int, data : int | list | tuple, echo = False) -> bool: return self.MemWRITE(id, addr, L2Bs(data), echo) @micropython.native def MemREAD(self, id : int, addr : int, length : int, echo = False) -> bytes: if addr >= 0 and id <= 239 and addr >= 0 and addr <= 0x4ff and length > 0 and length > 0 and length <= 247: if self.TxPacket(id, CMD_MemREAD, 0, W2Bs(addr) + B2Bs(length), echo)[1]: d, r = self.RxPacket(echo) if r: if d[4] == 0x20 and id == d[2] and (d[5] & (4 + 8 + 0x10 + 0x40)) == 0: return bytes(d[6:-2]) return None @micropython.native def MemREAD8(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, length, echo) if r != None: n = sum(iter_unpack('b' if signed else 'B', r), ()) return n if length > 1 else n[0] return None @micropython.native def MemREAD16(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, 2 * length, echo) if r != None: n = sum(iter_unpack('h' if signed else 'H', r), ()) return n if length > 1 else n[0] return None @micropython.native def MemREAD32(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.MemREAD(id, addr, 4 * length, echo) if r != None: n = sum(iter_unpack('i' if signed else 'I', r), ()) return n if length > 1 else n[0] return None @micropython.native def LOAD(self, id : int, echo = False) -> bool: if self.TxPacket(id, CMD_LOAD, 0, (), echo)[1]: if id != BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x22 else: return True; return False @micropython.native def SAVE(self, id : int, echo = False) -> bool: if self.TxPacket(id, CMD_SAVE, 0, (), echo)[1]: if id != BROADCASTING_ID: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x23 else: return True; return False @micropython.native def MotorREAD(self, id : int, echo = False) -> tuple: if self.TxPacket(id, CMD_MotorREAD, 0, (), echo): if id != BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return tuple([dat[6], tuple(n[0] for n in iter_unpack('<h', dat[7:-2])) if len(dat[7:-2]) > 0 and dat[4] == 0x24 else ()]) else: return None else: return () return None @micropython.native def MotorWRITE(self, id : int, opt : int, dat : (), echo = False) -> tuple: if self.TxPacket(id, CMD_MotorWRITE, opt, W2Bs(dat), echo): if id != BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return tuple([dat[6], unpack('<'+'h'*(len(dat[7:-2])>>1), dat[7:-2]) if len(dat[7:-2]) > 0 and dat[4] == 0x25 else ()]) else: return None else: return () return None @micropython.native def SystemREAD(self, id : int, echo = False) -> tuple: if self.TxPacket(id, CMD_SystemREAD, 0, (), echo)[1]: d, r = self.RxPacket(echo) if r: if len(d[6:-2]) == 13 and d[4] == 0x3b: return unpack('<IIIB', d[6:-2]) return None @micropython.native def SystemWRITE(self, id : int, data : (), echo = False) -> bool: if data[0] >= 0 and data[0] <= 239 and data[1] >= 0 and data[1] <= 7 and data[2] >= 0 and data[2] <= 2 and data[3] >= 1 and data[3] <= 255: d = self.SystemREAD(id, echo) if d: with self.__mutex: if self.TxPacket(id, CMD_SystemWRITE, 0xf, L2Bs(d[0]) + B2Bs(data), echo)[1]: d, r = self.RxPacket(echo) if r: return id == d[2] and d[4] == 0x3c and (d[5] & (4 + 8 + 0x10 + 0x40)) == 0 return False @micropython.native def ReBoot(self, id : int, echo = False) -> bool: if id != BROADCASTING_ID: if self.TxPacket(id, CMD_ReBoot, 0, W2Bs(0), echo)[1]: d, r = self.RxPacket(echo) if r: return d[2] == id and d[4] == 0x3d return False @micropython.native def FactoryReset(self, id : int, echo = False) -> bool: if id != BROADCASTING_ID: d = self.SystemREAD(id, echo) if d != None: if self.TxPacket(id, CMD_FactoryReset, 0, L2Bs(d[0]), echo)[1]: d, r = self.RxPacket(echo) if r: return d[2] == id and d[4] == 0x3e return False @micropython.native def FullScan(self) -> tuple: orgtimeout = self.__timeout orgbaudrate = self.__baudrate #baud=(3000000,2000000,1500000,1250000,1000000,625000,115200,57600) baud=(57600,115200,625000,1000000,1250000,1500000,2000000,3000000) self.__serial.timeout = 0.001 for b in baud: for id in range(240): self.__serial.baudrate = b r = self.MemREAD8(id, 0) print(f'baud:{b:7} id:{id:3} :', end='findn' if r else 'noner') self.__serial.timeout = orgtimeout self.__baudrate = orgbaudrate ########################################################## # test code ########################################################## if __name__ == "__main__": import time from pyPMX import * machine.freq(250000000) # set the CPU frequency to 250 MHz try: pmx = PMXProtocol() except: pass else: ID = 0 # reboot print('ReBoot=', pmx.ReBoot(ID, echo=True)) time.sleep(0.5) ''' # factory reset print('FactoryReset=',pmx.FactoryReset(ID)) time.sleep(0.5) ''' # load print('LOAD=', pmx.LOAD(ID, echo=True)) # save print('SAVE=', pmx.SAVE(ID, echo=True)) ''' # sys write print('SysWrite=',pmx.SystemWRITE(ID, (ID, pmx.SYSW_BAUD_115_2k, pmx.SYSW_PARITY_NONE, 20))) time.sleep(0.5) ''' # sys read r = pmx.SystemREAD(ID, echo=True) print('SysRead=', f'${r[0]:08x} ${r[1]:08x} ${r[2]:08x} {r[3]:d} ' if r else 'False') # dump memory print('dump') for addr in range(0, 800, 20): r = pmx.MemREAD(ID, addr, 20) print(f' MemREAD({addr}:{20})=', r.hex(':') if r else '!!!!!!!!!!!!!!!!!!!!!!!!') # gain r = pmx.MemREAD(ID, 0, 64) print('gainn MemREAD(0:64)=', unpack('<IIIIIIIIIIIIIIII', r) if r else '!!!!!!!!') # voltage limit r = pmx.MemREAD(ID, 76, 8) print('volt limn MemREAD(76:8)=', unpack('<HHHH', r) if r else '!!!!!!!!') # present value r = pmx.MemREAD(ID, 300, 24) print('present valn MemREAD(300:20)=', unpack('<hhhhhhhhhHHH', r) if r else '!!!!!!!!') # error stat r = pmx.MemREAD(ID, 400, 6) print('err statn MemREAD(0:400:7)=', unpack('<BBBxH', r) if r else '!!!!!!!!') print('preparation for pos ctrl') # ctrl off print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 1)) print(' MemWRITE8(502)=', pmx.MemWRITE8(ID, 502, 0b11111)) time.sleep(0.1) # goal angle print('start pos ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for ang in tuple(range(0, 32000, 500)) + tuple(range(32000, -32000, -500)) + tuple(range(-32000, 500, 500)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (ang,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) else: continue pass break else: print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) time.sleep(0.1) # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time print(' MemWRITE8(501)=', pmx.MemWRITE8(ID, 501, 2)) time.sleep(0.1) # goal angle print('start speed ctrl') print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ())) for spd in tuple(range(0, 2000, 50)) + tuple(range(2000, -2000, -50)) + tuple(range(-2000, 50, 50)): for n in range(10): r = pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (spd,)) if r != None: if len(r) > 0: print(f' MotorWRITE= {r[0]},', ''.join('{:6},'.format(k) for k in r[1]), 'stat=', pmx.status, end = 'r') pmx.MemREAD(ID, 400, 6) else: pass print(' MotorWRITE= !!!!!!!!!!!!!!!!!!!!!!!!') break time.sleep(0.005) else: continue pass break else: print() time.sleep(0.1) print(' MotorWRITE=', pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ())) del pmx print('fin')
なお「~/DX2LIB_v?.?/SampleCode/C/config.c」はID・ボーレート・動作モードの変更、テスト運転用のサンプルソースになりますので、Dynamixelの諸設定変更の際は利用してみてはいかがでしょうか。
¶RasPi5のconfig.txtに追記する情報
更にRaspberry Pi 5の場合ですが、Raspberry Piに搭載されたRTCが優先するようです。DXHATのRTCを使用する場合は以下を追記して下さい。
¶config.txtに追記する情報
[cm5] dtparam=rtc=off
DXHATはRaspberry Pi用のHATで、装備した主な機能は以下の通りです。
※Raspberry Pi 3/4/5や電源装置が別途必要です。
DXHATはRaspberry Pi用のHATで、装備した主な機能は以下の通りです。
※本製品は半田付け作業を要求します。
※Raspberry Pi 3/4/5や電源装置が別途必要です。
※慣性測定ユニット付の場合は12セット単位での見積及び受注とします。
品名 | 数量 | 型番・その他 |
DXHAT | 1個 | E179 |
スペーサ・ネジ類 | 1式 | スペーサー六角両メネジ M2.6 L8 x4 スペーサー 丸型中空 M2.6 L9 x4 スペーサー 丸型中空 M2.6 L8 x4 なべ小ねじ M2.6 L15 x4 低頭小ねじ M2.6 L5 x4 |
ターミナルブロック | 1個 | TB111-2-2-U-1-1 |
電源ボタンモジュール | 1個 | E191 (300mmハーネス付) |
商品番号 | BTE100 | |
PCBリビジョン | E179 | |
動作温度範囲 | 0~70℃ 結露なき事 | |
寸法 | 60x56.5x15mm 取り付け穴:58x49mm 4-φ3.0mm | |
重量 | 25g | |
外部電源 | DC6.5~24V (絶対最大定格 DC-0.3V/+30V) | |
内部電源 | DC5V | 外部電源からDCDCコンバータを介して生成 最大6A供給可 |
DC3.3V | Raspberry Piより供給 | |
RTC | DS3231M 電気二重層コンデンサによる簡易電源バックアップ | |
冷却ファン | 電源電圧 | DC5V |
最大送風量 | 2.2CFM | |
各I/F仕様 | RS-485 | 複信 半二重 最大通信速度 20Mbps 最大端末数 256台 ESD保護 ±15kV 絶対最大定格 -60~60V |
TTL | 複信 半二重 最大通信速度 12Mbps 最大端末数 32台 ESD保護 ±2kV 絶対最大定格 -0.5~6V | |
コネクタ等 | 2x20ピン パススルーソケット x1 2ピン5.08mmピッチランド x1 JST S4B-EH x4 JST S3B-EH x4 JST B3B-ZR x1 JST B2B-ZR-SM4 x1 4ピン2.5mmピッチランド x4 | |
環境配慮 | RoHS準拠, 鉛フリー半田 |
端子番号 | 信号名 |
1(N) | GND |
2(P) | VDD |
Pats Name | JST Parts Number |
基板用ヘッダー | B3B-ZR |
ハウジング | ZHR-3 |
ターミナル | SZH-002x-P0.5 |
圧接コネクタ | 03ZR-8M-P |
端子番号 | 信号名 |
1 | GND |
2 | PB |
3 | - |
No. | 端子名称 | I/O | No. | 端子名称 | I/O |
1 | 3V3 | I | 2 | 5V | O |
3 | GPIO2(SDA1) | I/O | 4 | 5V | O |
5 | GPIO3(SCL1) | I/O | 6 | GND | O |
7 | GPIO4(KILL) | O | 8 | GPIO14(TXD0/1) | I |
9 | GND | O | 10 | GPIO15(RXD0/1) | O |
11 | GPIO17(INT) | I | 12 | - | |
13 | - | 14 | GND | O | |
15 | - | 16 | - | ||
17 | 3V3 | I | 18 | - | |
19 | - | 20 | GND | O | |
21 | GPIO9(RXD4) | O | 22 | - | |
23 | - | 24 | GPIO8(TXD4) | I | |
25 | GND | O | 26 | - | |
27 | GPIO0(TXD2) | I | 28 | GPIO1(RXD2) | O |
29 | I/O | 30 | GND | O | |
31 | - | 32 | GPIO12 | I/O | |
33 | GPIO13 | I/O | 34 | GND | O |
35 | GPIO19(FAN) | I/O | 36 | GPIO16 | I/O |
37 | GPIO26 | I/O | 38 | GPIO20 | I/O |
39 | GND | O | 40 | - |
Pats Name | JST Parts Number |
基板用ヘッダー | B2B-ZR-SM4 |
ハウジング | ZHR-2 |
ターミナル | SZH-002x-P0.5 |
圧接コネクタ | 02ZR-8M-P |
端子番号 | 信号名 |
1 | 5V |
2 | オープンドレイン出力 |
端子番号 | 信号名 |
1 | GND |
2 | VDD |
3 | TTL Signal |
端子番号 | 信号名 |
1 | GND |
2 | VDD |
3 | RS-485 D+ |
4 | RS-485 D- |
端子番号 | 信号名 |
1 | 5V |
2 | COM |
3 | 3V3 |
4 | GND |
端子番号 | 信号名 |
1 | COM |
2 | GND |
3 | R_SDA |
4 | R_SCL |
端子番号 | 信号名 |
1 | 3V3 |
2 | GND |
3 | GPIO12(PWM0/SD4/DPI_D8/SPI5_CE0_N/TXD5/SDA5) |
4 | GPIO13(PWM1/SD5/DPI_D9/SPI5_MISO/RXD5/SCL5) |
端子番号 | 信号名 |
1 | GPIO19(PCM_FS/SD11/DPI_D15/SPI6_MISO/SPI1_MISO/PWM1) |
2 | GPIO16(FL0/SD8/DPI_D12/CTS0/SPI1_CE2_N/CTS1) |
3 | GPIO26(SD0_DAT2/TE0/DPI_D22/SD1_DAT2/ARM_TDI/SPIO5_CE1_N) |
4 | GPIO20(PCM_DIN/SD12/DPI_D16/SPI6_MOSI/SPI1_MOSI/GPCLK0) |
Raspberry Piに適用するOSや環境の構築状況によって設定方法や挙動が変わります。ここではそれらの詳説を省略しますので、公式サイト等などから情報を補完して下さい。
HATの各機能は主にconfig.txtに必要な情報を追記する事で活性化されます。なおconfig.txtから別のファイルをincludeしている場合もありますので、それらも含めて全ての設定ファイルを精査する必要があります。
CN3と四隅の取り付け穴の位置をRaspberry Piのピンヘッダと取り付け穴に一致させ装着します。
HATとRaspberry Piの電子部品と基板が接触しないよう四隅の取り付け穴に同じ高さのスペーサを設置し、相互の基板を平行に保持します。
なお実測でRapspberry Pi 4の場合は8mm、Raspberry Pi 3の場合は9mm以上のスペーサが必要ですが、10mm以下でないとピンヘッダが嵌合しません。
CN1にコネクタやケーブルを装備し外部電源を供給しますが、CN1から外部電源を供給してもRaspberry Piへの給電は開始されず、CN2に装着するプッシュモーメンタリなスイッチで電源供給の操作を行う必要があります。
Raspberry Pi上でOSが動作している間に電源の供給が突然断たれる事は大抵の場合において想定されておらず、一般的にはpoweroffコマンドを実行し、シャットダウン処理が完全に完了した頃合いを見計らって電源の供給を断つといった操作が強いられます。もしOSが動作している時に突如電源の供給が断たれてしまうと、タイミングによってはファイルシステムが破損し、次回以降OSが正常に起動しなくなります。
HATには電源の操作の煩雑さを簡素化する機能が備わっており、CN2に装備したプッシュスイッチの操作によって、電源ON・OSへのシャットダウン励起/OSの終了と同時に電源OFF・強制電源OFFといった処理が安全に行えます。
config.txtに追記する情報
[all] dtoverlay=gpio-shutdown,gpio_pin=17 dtoverlay=gpio-poweroff,gpiopin=4,active_low=1
なおubuntuをGUIで起動している場合は、プッシュスイッチを押下するとログアウトのプロンプトが表示され、タイムアウトするかダイアログのpower offを選択するまでシャットダウンが行われません。プッシュスイッチ操作によってシンプルにシャットダウン処理を行わせるには、GNOMEの設定を変更する必要があります。なおログインしていない状態ではこの設定は効果がありません。
gsettings set org.gnome.SessionManager logout-prompt false
ちなみに付属の電源ボタンモジュールは3線で構成されていますが、電源のON/OFFだけであればPBとGND信号の2本あれば事足ります。残りの1本は1kΩの抵抗を介してプッシュスイッチに内蔵されているLEDのアノードに接続されていますので、CN2-3に5Vや3.3V程度の電圧やGPIOの信号を接続するとLEDを点灯させる事ができます。
外部電源の逆極性への耐性はないため、絶対に極性を間違えてはならない。 HATを装着した後は、Raspberry Pi本体に備わっているMicro USBないしUSB Type Cコネクタから電源を供給してはならない。 |
HATに装備された冷却ファンはRaspberry Piへ外気を送り込む事で冷却を促し、発熱によってSoCのクロックが下がりパフォーマンスが低下する事を低減させるのが目的です。
合わせてRaspberry Pi上の主要チップにヒートシンクを装着すれば、冷却効果を更に高める事が期待できます。
エアフローはHAT上面からファンで吸気し、HATとRaspberry Piの間隙から排気を想定しています。もしケースなどに封入する際は、吸気口と排気口を適宜設けたり、HAT上の冷却ファンを取り外してケース側でエアフローを設計して下さい。
冷却ファンのON/OFFはRaspberry PiのGPIO19を用いて行われ、内部温度と設定温度に応じてON/OFFを自動的に制御する事ができます。
config.txtに追記する情報
[all] dtoverlay=gpio-fan,gpiopin=19,temp=50000temp=50000は50℃で冷却ファンをONする意味で、摂氏度の1000倍の値を指定
設定温度を10℃下回るとファンはOFFする
なおRaspberry Piの発熱はOSや実行しているアプリケーションの負荷等に依存します。またRaspberry Piの性能限界までオーバークロックすると冷却しきれない場合があります。
冷却ファンへの接触防止措置を行っていないため、巻き込まれ等に注意。 |
Raspberry Piの時刻はNTPサーバなどを利用してOSの起動時に更新するのが一般的なようです。しかし時刻を取得できなければ、OSは不正確な時刻を元に動作する事になります。
HATはI2C接続のRTC(I2Cスレーブアドレス0x68)を装備しており、外部電源が断たれた場合でも1日程度は電気二重層コンデンサがRTCの電源を供給します。電気二重層コンデンサを満充電させるには、少なくとも半日程度は電源ON状態を維持して下さい。
config.txtに追記する情報
[all] dtparam=i2c_arm=on dtoverlay=i2c-rtc,ds3231
hwclockコマンドを使う事でRTCが機能しているかの確認ができます。
#print hardware clock hwclock -r #set the hardware clock from the current system time hwclock -w #set the system time from the hardware clock hwclock -s
なおubuntuであればNTCとRTCそしてシステムクロックがうまく連動しますが、他のOSの場合は初期状態においてRTCの扱いが異なる場合があります。思ったような挙動をしない場合は、こちらの情報を参考にして下さい。
更にRaspberry Pi 5の場合ですが、Raspberry Piに搭載されたRTCが優先するようです。DXHATのRTCを使用する場合は以下を追記して下さい。
RasPi5のconfig.txtに追記する情報
[cm5] dtparam=rtc=off
Raspberry PiのUSBポートにDXHUB等を接続してDynamixel用のI/Fを増設する事も可能ですが、HATにはDynamixelと通信するためのRS-485とTTLの2種類のI/F(CN5~8/CN9~12)が装備されており、CN1に供給される電源はDynamiel用の電源として供給されます。
Raspberry Piが持っているUARTを使用する事で、USB接続したシリアルI/Fの一般的な1msのレイテンシを超越した運用ができます。基本的にはTTL2DXIFと同等の回路が装備されていますので、注意事項はそれと同様となります。
使用するUARTはJP1, JP2を半田ジャンパする事で選択しますが、Raspberry PiのGPIOは複数の機能を兼用しているため、使用環境に応じて適宜設定します。またUARTのベースクロックを64MHzに設定する事で、最大4Mbpsの通信速度に対応できます。
config.txtに追記する情報
[pi3] init_uart_clock=64000000 dtoverlay=uart0=on dtoverlay=pi3-miniuart-bt [pi4] init_uart_clock=64000000 #UART1を活性化 dtoverlay=uart1,txd1_pin=14,rxd1_pin=15 #UART2を活性化 dtoverlay=uart2,txd2_pin=0,rxd2_pin=1 #UART4を活性化 dtoverlay=uart4,txd4_pin=8,rxd4_pin=9使用するポートのみを活性化する事を推奨。Linux上でのデバイス名は概ねttyAMAのプレフィクスで始まる。
またRaspberry Pi 3/Zeroの場合はcmdline.txtに記述されている「console=serial0,115200」を削除する事。
対応するデバイスは以下の通りです。
Device Name |
RS-485ないしTTL I/Fを装備したDynamixel DX・AX・RX・EX・MX・X・PRO・P・USS3・DXMIO |
なおMolex社のコネクタが装備されたデバイスを接続する場合は、Robot Cable-X3P 180mm (Convertible)ないしRobot Cable-X4P 180mm (Convertible)を使用して下さい。
外部電源の電流容量が小さいとDynamixelに過度な負荷が生じた際にRaspberry Piに供給される電源に影響が及ぶ。 Dynamixelから発生した逆起電力によって電源電圧が上昇た場合、HAT上の電子部品の絶対最大定格電圧を超えて破損する可能性がある。 接続するDynamixelの総容量は、外部電源のラインに挿入されたFETとFR-4のガラスエポキシ基板の許容電流容量を超えない事。 様々な事象の想定範囲を広げ、不慮の故障に至らないようシステムを設計する事。 |
Linux上で動作するDynamixelを制御するプログラムを作る際は、別途用意したライブラリを用いる事でシリアル通信やプロトコルを意識せずにコーディングができます。
APIに対して対象デバイスのID・番地・値といった引数を与えるだけでコントロールテーブルの読み書きが実現でき、マルチスレッド等で複数のデバイスに対して個別にアクセスする様なアプリケーションも容易に構成できます。
Dynaimxelには2種類のプロトコルが存在しますが相互非互換であるため、プロトコルに応じて2種類のライブラリを用意しています。
Dynamixel Protocol V2用ライブラリを使用する際の手順を少しだけ紹介しておきます。全てターミナル上での操作となります。
sudo apt install build-essential unzip
cd ~ wget https://www.besttechnology.co.jp/download/DX2LIB_V2.9.zip unzip DX2LIB_V2.9.zip
cd ~/DX2LIB_v2.9/DX2LIB/ bash ./build_dx2lib.sh
以上でライブラリファイルが自身の環境用に再構築され、同時に各サンプルソースディレクトに実行に必要なファイルがコピーされます。
Pythonで記述されたサンプルソースであれば多少の改変で動かせますので試してみます。
cd ~/DX2LIB_v2.9/SampleCode/Python/ nano smpl1.py
通信ポートとボーレートは自身の環境に依存するため、適宜修正して保存します。例としてuartが「/dev/ttyAMA1」、ボーレートが「2Mbps」、IDが「1」とした場合は、ソースコード中の以下の変数に代入している部分を修正して保存します。
COMPort = b'/dev/ttyAMA1' Baudrate = 2000000 TargetID = 1
最後にsmpl1.pyをPythonで読み込ませて実行させます。
python3 smpl1.py
もしpermission deniedと言われた場合は、adduserを使ってdialoutグループに自身のユーザーを登録してから実行し直しましょう。
sudo adduser $USER dialout
Dynamixelとの通信に成功すると、DynamixelのLEDが1秒間点灯します。他のサンプルソースもほぼ同様の変更を加えることで利用できます。
なお「~/DX2LIB_v?.?/SampleCode/C/config.c」はID・ボーレート・動作モードの変更、テスト運転用のサンプルソースになりますので、Dynamixelの諸設定変更の際は利用してみてはいかがでしょうか。
またRaspberry Piの動作条件によってはAPIがタイムアウトエラーを返す場合があります。その際はDX2_SetTimeOutOffset APIを使ってタイムアウトエラーが発生しない程度の時間を設定すると良いでしょう。
I2CバスにはRTCが既に接続されていますが、I2C接続のデバイスをさらに追加できるようにバスリピータを介してJ2にI2Cバスが接続されています。
I2Cバスに接続する外部機器の電圧に応じて、J1のCOMに供給する電源を5Vか3.3Vから選択します。同時にI2Cバスの信号ラインは4.7kΩの抵抗を介して選択したCOMの電圧にプルアップされます。
なおJ2を使用しない場合は、COMを3V3に接続しておく事を推奨します。
J3/J4に接続された一部のGPIO端子だけがHATに配線されています。スペーサの長さ調整が必要ですが、エレベーテッド ソケットストリップ等を挿入する事でHATの表面からRaspberry PiのGPIOを全て引き出す事が可能です。
記載された事項以外にも、経験を踏まえた危険回避方策を講じる事。 安全に配慮しない場合は人命や財産を失う恐れがある。 また従わない場合は保証対象外となる。 |
def __init__(self, port : serial.Serial | str, baudrate = 57600, timeout = 0.05, mutex = None): port : serial.Serial | str pySerial or Device name def __init__(self, port : serial.Serial | str, baudrate = 57600, timeout = 0.05, mutex = None): port : serial.Serial | str pySerial or Device name
# (C) 2024 BestTechnology
def __init__(self, port, baudrate = 57600, timeout = 0.05, mutex = None): if isinstance(port, serial.Serial): self.__serial = port self.__baudrate = port.baudrate self.__timeout = port.timeout else: print(port, baudrate, timeout) print(type(port)) self.__serial = serial.Serial(port, baudrate = baudrate, timeout = timeout) self.__baudrate = self.__serial.baudrate self.__timeout = self.__serial.timeout if mutex == None: self.__mutex = threading.Lock() else: self.__mutex = mutex def __reconfig(self): self.__serial.baudrate = self.__baudrate self.__serial.timeout = self.__timeout
self.__reconfig() r = self.Read(id, addr, 2 << (length - 1), echo) r = self.Read(id, addr, 4 << (length - 1), echo) def __init__(self, port, baudrate = 57600, timeout = 0.05, mutex = None): if isinstance(port, serial.Serial): self.__serial = port self.__baudrate = port.baudrate self.__timeout = port.timeout else: print(port, baudrate, timeout) print(type(port)) self.__serial = serial.Serial(port, baudrate = baudrate, timeout = timeout) self.__baudrate = self.__serial.baudrate self.__timeout = self.__serial.timeout if mutex == None: self.__mutex = threading.Lock() else: self.__mutex = mutex def __reconfig(self): self.__serial.baudrate = self.__baudrate self.__serial.timeout = self.__timeout
self.__reconfig() r = self.Read(id, addr, 2 << (length - 1), echo) r = self.Read(id, addr, 4 << (length - 1), echo) for addr in range(0, 700, l): ''' trace = [] tb = ex.__traceback__ while tb is not None: trace.append({ "filename": tb.tb_frame.f_code.co_filename, "name": tb.tb_frame.f_code.co_name, "lineno": tb.tb_lineno }) tb = tb.tb_next print(str({ 'type': type(ex).__name__, 'message': str(ex), 'trace': trace })) ''' import sys import platform print(sys.version) try: dx = DXLProtocolV2('\\.\COM12', 3000000) except: pass else: th1 = Thread(target = func1, args = (dx,)) th2 = Thread(target = func2, args = (dx,)) th1.start() th2.start() th1.join() th2.join() del th1, th2, dx try: dx = DXLProtocolV1('\\.\COM12', 57600) except: dx = None else: th3 = Thread(target = func3, args = (dx,)) th3.start() th3.join() del th3, dx
r = self.Read(id, addr, 2 * length, echo) r = self.Read(id, addr, 4 * length, echo) r = self.Read(id, addr, 2 * length, echo) r = self.Read(id, addr, 4 * length, echo)
ひとまずPicoSHIELD+Raspberry Pi Pico上で動くように修正してみた。オーバーロックしているのはご愛敬として、マルチスレッドを除外した程度で実質PC版と大差無い。@micropython.viperを多用した時はかなり高速化できていたのだが、ここはコードの互換性を優先。
¶ひとまずPicoSHIELD+Raspberry Pi Pico上で動くように修正してみた。オーバーロックしているのはご愛敬として、マルチスレッドを除外した程度で実質PC版と大差無い。@micropython.viperを多用した時はかなり高速化できていたのだが、ここはコードの互換性を優先。
様々なアプリケーションにおいてDYNAMIXELでつまづかないよう、最低限運転できるところまでの流れを本ページにまとめた。PCの操作やOSについても説明をしなくてはならない事も多々あるが、ここでは「ひとまず」動かせるところまでを一気に紹介する事で、分かった気になってもらう事に主眼を置いた。
そのような中においても多少なりともTipをちりばめてあるので、最終的にはそれらを踏まえたシステムを構築して欲しい。
縦に長くなって閲覧しにくいかもしれないが予めご了承の程。また別ページへのリンクをクリックするとブラウザ上ではページが切り替わるが、何度も往来しているうちに元に戻るのが億劫になる事がある。リンク先を参照する際は「Ctrl」キーを押しながらクリックして新規タブで開けば、多少なりとも閲覧効率が良くなるかも知れない。
※ここに記述された内容は断り無く更新される。
DYNAMIXELは主に2種類のI/Fが規定され、3ピンコネクタ版を俗にTTL版、4ピンコネクタ版をRS-485版と呼んでおり、電気的には相互互換性は無い。ほとんどのDYNAMIXELにはいずれかのタイプのコネクタが2個装備されている。
まずこの2種類のI/Fのコネクタを紹介する。
端子番号 | 信号名 | |
1 | GND | 電源のマイナス側 シリアル通信のGNDと共有 |
2 | VDD | 電源のプラス側 大半のDYNAMIXELは12V |
3 | TTL Signal | シリアル通信の信号 双方向 |
端子番号 | 信号名 | |
1 | GND | 電源のマイナス側 シリアル通信のGNDと共有 |
2 | VDD | 電源のプラス側 大半のDYNAMIXELはDC12V |
3 | RS-485 D+ | シリアル通信のプラス側差動信号 双方向 |
4 | RS-485 D- | シリアル通信のマイナス側差動信号 双方向 |
紹介したTTL及びRS-485の2種類のI/Fは一般的なPCには標準装備されていないため、USBポートを介して増設するための装置が用意されている。複数のI/Fに対応していたり、いずれか1つのI/Fにのみ対応していたり、電源の供給を行えたりとラインナップが複数あるため、以下に現行品のみの比較表を示す。
USB2RS485 dongle | USB2TTL dongle | USB2DXIF dongle | USB2DXIF | Starter Kit A | Starter Kit B | PicoSHIELD | DXHUB | U2D2 | |
Manufacture | BestTechnology | ROBOTIS | |||||||
USB | USB2.0 FS USB Type-A | USB2.0 FS USB Type-C | USB2.0 FS USB micro-B | USB2.0 FS USB Type-C | USB2.0 FS micro USB Type-B | USB2.0 HS USB Type-C | USB2.0 HS micro USB Type-B | ||
FTDI Chip | FT234X | FT231XS | FT234X | RP2040 | FT232H | ||||
Max DTR Rate [Mbps] | 3 | 3 | 12 | 6 | |||||
DYNAMIXEL I/F | RS-485 x1 | TTL x1 | RS-485 x1, TTL x1 | RS-485 x1, TTL x1 | RS-485 x5, TTL x5 | RS-485 x4, TTL x4 | RS-485 x1 | RS-485 x6, TTL x6 | RS-485 x1, TTL x2 |
Isolation | Yes | No | Yes | No | |||||
Dimension [mm] | 43.2x13.0x8.5 | 18.0x12.7x9 | 40x40 | 51x23.5 | 52x33x15 | 48x18x14.6 | |||
Other spec. | Small | Tiny | Power distribution | Plastic enclosure | |||||
Software compatibility | DYNAMIXEL Wizard 2.0, R+2.0, DXLIB, DX2LIB, DYNAMIXEL SDK |
ここで紹介していない古いI/F製品は概ね旧来のDYNAMIXELを前提としており、装備されているコネクタがmolex社製のものが主である。もちろんConvertible Cableを仲介させる事で現行モデルのDYNAMIXELにも接続できるので、持っているのであれば買い直す必要は無い。
現時点ではAX・MX・X・Pシリーズから選定する事になるが、シリーズごとの大雑把な特徴を紹介しておく。
MXとXシリーズのストールトルクと無負荷最大回転数をモデルごとにプロットしたグラフを紹介しておく。
アプリケーションに要求されるトルクで選定するのが一般的なのだが、DYNAMIXELの大半のモデルは定格トルクではなくストールトルクを謳っている。ストールトルクは物理的にこの値を超えて出力されることは無い値(瞬時最大トルクに近い)であり、連続的に使って良いという意味では無い。
なおここではあえてXシリーズ以降を前提とするので、AXやMXシリーズまたは廃盤品を使用する場合は差異を各自で深掘りした上で適宜読み替えてもらうものとする。また廃盤品を含む全ラインナップの一覧はこちらに掲載した。
各USB I/Fを使用した場合の実態配線を紹介する。
いずれも複数のDYNAMIXELのコネクタをお互いに並列接続し、更にそこへUSB I/Fにて増設されるTTLないしRS-485 I/Fの信号と外部電源を接続する事が主な目的である。
なおmolex社のコネクタを装備したDYNAMIXELを使用する場合はJST社とmoelx社のコネクタを変換するConvertible Cableが必要だが、ここではXシリーズを前提で書き進めるので詳説しない。
3ピンのTTLと4ピンのRS-485 I/Fのコネクタが複数備わっている。更にDYNAMIXELへ電源を分配供給する機能があり、その電源の入力用コネクタが装備されている。各装置をDXHUBへ接続すると基本的に接続が完結してしまう。
同一電源で構わないのであればTTLとRS-485 I/Fを装備したDYNAMIXELを同時に運用できる。
4ピンのRS-485 I/Fのコネクタが1つのみ装備されており、DYNAMIXELへの電源の供給機能は無い。別途DXSharingBoard(RS485)等の分配基板を用意する事でDYNAMIXELへの電源の供給と複数台のDYNAMIXELの接続ができる。
この例ではPCとDYNAMIXEL間の距離はRobot Cable X4Pに支配されるが、USBドングルとPC間の接続を市販のUSB延長ケーブル等で中継する事で延長する事ができる。
更に正しく処置したRS-485ケーブルを作成する事で、USBドングルとDYNAMIXEL間の距離を延長できる。
3ピンのTTL I/Fのコネクタが1つのみ装備されており、DYNAMIXELへの電源の供給機能は無い。別途DXSharingBoard(TTL)等の分配基板を用意する事でDYNAMIXELへの電源の供給と複数台のDYNAMIXELの接続ができる。
この例ではPCとDYNAMIXEL間の距離はRobot Cable X3Pに支配されるが、USBドングルとPC間の接続を市販のUSB延長ケーブル等で中継する事で延長する事ができる。
USB2RS485 dongleとUSB2TTL dongleの機能を合わせ持っており、3ピンのTTL I/Fと4ピンのRS-485 I/Fのコネクタがそれぞれ1つずつ装備されている。
そもそもUSB2RS485 dongleとUSB2TTL dongleとUSB2DXIF dongleはコネクタの実装状況が異なる以外は全く同じ代物であるので、USB2RS485 dongleないしUSB2TTL dongleを参考に接続が行える。
4ピンのRS-485 I/Fのコネクタが1つ、3ピンのTTL I/Fのコネクタが2つ装備されており、DYNAMIXELへの電源の供給機能は無い。別途U2D2 PHBやDXSharingBoard等の分配基板を用意する事でDYNAMIXELへの電源の供給と複数台のDYNAMIXELの接続ができる。
TTLとRS-485 I/Fを同時に運用できる。
なおU2D2 PHBの電源供給用のジャックはROBOTIS社のキットに同梱されるSMPSのサイズに合わせてあり、日本国内で多く流通するものには適合しない。
TTL I/Fのシリアル信号は極めて微弱かつ脆弱で、距離を延長する事ができないまでか、DYNAMIXELの電気回路を容易に故障させる可能性すらある。弊社ではTTL I/Fの選択肢しかないモデルを除き、堅牢なシステムにおいてはRS-485 I/Fを搭載したモデルを強く推奨する(モデル名のサフィックスに「-R」と記載)。
また単体製品及びDYNAMIXELに同梱されているRobot Cableはケーブルを製作せずともすぐに接続できる事を念頭に用意されており、一般的に考え得る電気的な問題を解消できるものでは無い。
特にRS-485 I/Fの実力を発揮させるためには既成のRobot Cableは適さないため、電源容量と延長距離に対応したケーブルの製作を推奨する。
RS-485の終端抵抗の処置も行っておきたいところだ。
先の図では電源装置をSMPS等と表記しているが、任意電圧に設定できる電源装置やバッテリを負荷が要求する電圧と電流に合わせて用意する。しかしDYNAMIXELに内蔵されたモータの回生電力によって電圧が上昇してしまう事までは注意が行き届かないと思う。特に固定出力の一般的なスイッチングACアダプタには出力電圧と電流は規定されているが、出力側の耐圧やシンク能力までを明記しているものはまず無い。少なくとも誘導性負荷に対応しているものを選定しないと容易に故障する。
また電源からの距離をかせぐための延長ケーブルは、平行線ではなくツイストペアケーブル(より対線)を使うべきだ。
回路上PC・USBポート・USB I/F・DYNAMIXELの全てのGNDが共通になっている必要があるが、DYNAMIXEL側で電流サージが発生したりPCとDYNAMIXELに供給されている電源自体に大きな電位差があったりすると、GNDの電位が共通である事による問題が生じる場合がある。
大抵のPCのUSBポートには過電流保護機能が備わっているので、コネクタの抜き差し時に発生する電流サージ程度には耐性がある。しかしモータから生じるサージとなると、保護回路では太刀打ちできないレベルのものが発生する事があり得る。またUSBポートの過電流保護機能が働くとUSBポートが一時的に機能を失い、それを想定していないプログラムやOSであれば容易に暴走する。
更に各装置に供給される電源の電位差はサージ過電流以上に厄介で、瞬間的なものではなく定常的に過電流状態が維持されてしまい、あらゆる装置の永久破壊につながる。
いずれの問題にも幾度となく遭遇し、壊したPCやI/Fが何台ある事か。。。
そういった諸問題を解消する目的で、USB2RS485 dongle・USB2TTL dongle・DXHUBはいずれもUSBポートとDYNAMIXEL用I/F及び電源の間を電気的に絶縁している。
なお以後の解説では接続の容易さを買って絶縁機能を持ったDXHUBを使用する。
DYNAMIXELに関係するID番号・通信速度・モデル番号・コントロールテーブル・ファームウェアといったキーワードがあるが、大半はDYNAMIXELの外観から判別できない内部情報であり、後述するPC用のツールを用いる事で初めて認識できる。
電源・電源ケーブル・USBケーブルは各製品に標準添付されていないため、必要に応じて別途調達する。
接続は先に紹介したDXHUBの場合の通りとなるが、初期設定の場合は混乱を避けるため1台のみのDYNAMIXELを接続する。
DXHUBをPCのUSBポートに接続してデバイスドライバのインストールを行った後に、こちらの情報を元に仮想COMポートの設定を予め変更しておく事を推奨する。
またDXHUBの裏側が不用意に何かに触れてショートすると取り返しの付かない事になるため、予め四隅の取り付け穴にスペーサ等を取り付けて端子等がショートしないようにしておくと良いだろう。
DYNAMIXEL Wizard 2.0はDYNAMIXELとコミュニケーションを行うソフトウェアで、主に以下の機能を有する。
DYNAMIXELの専用通信プロトコルを用いて対象のDYNAMIXELのコントロールテーブルにアクセスしているに過ぎないが、このようなツールを使用しない場合はちょっとした設定変更をするにしても自前でプログラムを作らざるを得ない。ROBOTIS社のサイトからダウンロードしてインストールしておく。
なおDYNAMIXEL Wizard 2.0はインターネットを介してツール本体とファームウェアの更新版を取得する事ができる。ここでは詳説しないが、インターネットに接続された環境でDYNAMIXEL Wizard 2.0を起動した直後に自動的に更新がないかを確認し、必要があれば更新するか否かを問い合わせてくる。Install Nowボタンを押すと更新が開始されるが、特に適用する必要が無いと判断したらInstall laterボタンを押す事で更新はキャンセルされる。
インストールしたDYNAMIXEL Wizard 2.0を起動したらツールバーのOptionsボタンをクリックし、開いたOptionsダイアログボックスの左側のScanをクリックする。
以下の項目を設定してOKボタンを押すと、DYNAMIXEL Wizard 2.0の検索条件の期設定はひとまず完了である。次回起動した際も本設定が反映される。
なお本設定は出荷時のDYNAMIXEL Xシリーズを対象としているため、後々のDYNAMIXELの設定等によっては以下のように適宜変更する。
DXHUBの電源スイッチをONにして電源の供給を開始すると青色のLEDが点灯し、DYNAMIXELに装備されたLEDが0.5秒だけ点灯する。その後DYNAMIXEL Wizard 2.0のツールバーのScanボタンをクリックすると、検索条件の設定に従って検索が開始される。検索中のダイアログボックスにはプログレスバーが表示され、見つかったDYNAMIXELはダイアログボックスとメイン画面の画面左側のCOMポート番号と通信速度のツリーの配下に逐次列挙される。
Select baudrate to scan.の全ての通信速度がチェックされていると検索の終了までにかなりの時間がかかるが、検索中に目的のDYNAMIXELが見つかったり途中で中断する場合はSkipボタンを押す。
DYNAMIXEL Wizard 2.0は検索によって見つかったDYNAMIXELのみを操作対象とするため、検索は起動直後に毎回強いられる。検索条件の設定のSelect baudrate to scan.のチェックを減らしたり、Set ID range to scan.のStat及びEndのID番号を適宜変更する事で検索時間を短縮できる。
もし検索で見つからない場合は、検索条件の設定が対象のDYNAMIXELに合致しない・COMポートが間違っている・電源が入っていない・いずれかの装置の故障等の理由が考えられるが、原因を特定するのに少々難儀する場合もある。
出荷時状態のDYNAMIXELの動作モードは大抵「位置決め制御」モードとなっており、任意の位置を指示する事でDYNAMIXELのサーボホーンの位置がそれに伴って移動する。DYNAMIXEL Wizard 2.0では以下の手順と操作で位置を指示できる。
この手順で思ったようにDYNAMIXELのホーンが動かない場合は、DYNAMIXELが故障している可能性が高い。
複数のDYNAMIXELを同時に運用する場合は、個々に異なるID番号を設定しておかなくてはならない。通信プロトコルの仕様では複数のDYNAMIXELのID番号が同じだと個々を識別できないからである。
DYNAMIXELのデフォルトの通信速度は57600bpsだが、軸数が増えたりタイトな通信をし始めるとトラフィックが渋滞し、体感的にも応答が遅いと感じたりする。高い通信速度は様々な環境に影響されやすいため最高速まで設定する必要は無いが、1000000bps程度であれば経験的に安定して使用できる速度と考える。以下に通信速度を変更する手順を示す。
Value | 通信速度[bps] |
0 | 9,600 |
1 | 57,600 |
2 | 115,200 |
3 | 1,000,000 |
4 | 2,000,000 |
5 | 3,000,000 |
6 | 4,000,000 |
7 | 4,500,000 |
プログラミングの知識が無ければ何も始まらないし、それ相応の環境を整える必要もある。
ここでは弊社が提供するWindows向けの簡易的な統合環境と専用のライブラリを用い、C言語でコーディングする。コンパイル後に生成されるプログラムは、いずれもCUIベースである。
DYNAMIXELは専用の通信プロトコルでのみ通信できるが、通信プロトコルを処理するプログラムを自前でコーディングするのにはそれ相応の知識と労力が要求される。その手間を軽減させる目的で提供しているのがDynamixel Protocol 2 Library(DX2LIB)である。
他にもこういった目的のライブラリはあるが、DYNAMIXELの主要な機能を利用する際にモデルやコントロールテーブルの差異をほぼ意識しなくて済む追加APIをDX2LIBは持っている。追加APIを使用している限りユーザーはCOMポート番号・通信速度・ID番号程度を把握していれば良く、最終的には少ないソースコード量で大抵の処理が行えると謳っている。また各APIはコンパイル済みのDLLに含まれているので、自分のプログラムから呼び出す手段さえあれば言語は問わない。
見ての通り追加APIのソースでは大した事はしておらず、モデルによって異なるアイテムの変換や手順を強いられるアイテムの手順操作、物理値とアイテム間の単位変換、複数IDの一括処理に終始しているだけである。しかし追加APIを使わない場合は、コントロールテーブルのアイテムの機能を習熟する段取りから始めざるを得ないので、説明を端折るにはありがたい 。
なおプロトコルV1.0用のライブラリも用意されているので、どうしてもDX/AX/RX/EX/MXシリーズを動かしたいという場合は所々読み替えて対応してもらいたい。
また我々としてはプロトコルV1.0とV2.0を混在させた運用は危険と判断しているのでライブラリはプロトコルのバージョン別に用意しているが、DYNAMIXEL SDKでは同時に運用できる事になっている。
こちらより最新のアーカイブファイルをダウンロードし、適宜解凍する。
以後に紹介するGCC Developer Liteでコーディング・コンパイル・実行には、ライブラリに含まれる次の4つのファイルがソースコードファイルを保存したフォルダと同じ場所に配置されている事が前提となる。
ファイル名 | 説明 |
dx2lib.h | 各APIのプロトタイプ宣言等を含むヘッダファイル |
dx2memmap.h | コントロールテーブル上の主要アイテムのアドレスが定義されたヘッダファイル |
dx2lib_x32.dll | ライブラリ本体が含まれた32ビット版DLL |
dx2lib_x64.dll | ライブラリ本体が含まれた64ビット版DLL |
このうち「dx2memmap.h」は一部のDYNAMIXELのコントロールテーブルに配置されたアイテムのアドレスを記述しているファイルなのだが、アイテムのアドレスを指定してアクセスしない限り必須ではない。また32ビット版と64ビット版のDLLはコンパイル時の条件によっていずれか一方で構わない。その他のファイルは一切必要ない。
GCC Developer Liteはマイコンボード製品向けにプログラムを手っ取り早く動かせるツールとして提供しているが、ここではWindows上で動作するプログラムをコーディング及びコンパイルする事が目的となる。
最近の統合環境の類いに比べたら極めて少ない機能しか備えておらず正直なところ今更な感じではあるのだが、無償かつ簡単な設定でソースコードの編集とコンパイルができるという理由だけで選定した。
ここではWindows向けの実行プログラムを生成する事が目的なので、ここから「基本パック」と「WINパック」をダウンロードする。手順を間違えるとインストールできないので、こちらを読んでから作業する。
後述のソースコードを使用する際に必要な操作を紹介する。
まずブラウザから該当のソースコードをクリップボードにコピーし、GCC Developer Liteのエディタウィンドウ上にペーストする。ソースコード中のCOMポート番号の部分("\\.\COM3")の数値を自身の環境に合わせて書き換えたら編集作業は完了である。
編集中のソースコードをファイルとして保存しておけば、後々再度読み出して再利用できる。保存するには「ファイル(F)」メニュー内の「名前を付けて保存(A)...」をクリックし任意の場所と名前で保存する。なおファイル名のフルパスに全角文字・機種依存文字・空白が含まれているとコンパイルに支障をきたすので、基本的には半角の英数字で構成しておく事。
64ビット版のWindows上で32ビット版のプログラムは実行できるが、その逆はできない。32ビット版と64ビット版のどちらを選んでも大きな違いは感じないと思うが、ここではどちらのOSでも実行できる32ビット版のプログラムを生成する事にする。
コンパイルしたいソースコードを開いている状態で、「ツール(T)」メニュー内の「コンパイルオプション(O)」をクリックしコンパイルオプションダイアログボックを開く。上端の設定リストをドロップダウンし「Windows x86 (Console)」を選んでOKボタンを押すと、32ビット版のコンパイル条件の設定は全て完了する。
ちなみに「Windows x64 (Console)」を選ぶと64ビット版の実行ファイルを生成するためのコンパイル条件となる。
なお後述のビルドやデバッグ情報付きビルドを行った既存のソースコードファイルを開き直した場合、「保存済みの環境設定ファイルが見つかりました。」というメッセージダイアログボックスが表示される。
このメッセージは過去にコンパイルオプションを設定してコンパイルされた事を意味し、OKボタンを押す事でコンパイラオプションを設定したのと同等の扱いとなる。
後述のライブラリを利用したソースコードをコンパイルするには、ライブラリに含まれる4つのファイルが必要となる。それらファイルを予めソースコードを保存したフォルダにコピーしておく事を忘れないように。
コンパイラオプションが正常に設定されていれば、「コンパイル(C)」メニュー内の「ビルド(B)」が利用できるようになる。「ビルド(B)」をクリックする(もしくは「F9」キーを押す)とコンパイラが呼び出され、問題が無ければソースコードから実行可能なプログラムが同じフォルダの中に生成される。
例えばソースコードのファイル名が「test1.c」ならば、「test1.exe」というファイル名で実行ファイルが生成される。実行ファイルが生成されたら、そのファイルをエクスプローラー等から開いて実行する。
実行中はコマンドプロンプトが表示され、標準出力へ何かしらのメッセージが出力される。
なおエクスプローラーから起動したプログラムは、メッセージを確認する間もなく終了と同時にコマンドプロンプトが閉じてしまう。メッセージを確認する必要がある場合は予めコマンドプロンプト(cmd.exe)を起動しておき、その中でコンパイルしたプログラムを実行すれば終了時に勝手に閉じることはない。
ビルドと実行と大差は無いが、コンパイル後にデバッガを介してプログラムを実行する。ここではデバッガを使ってデバッグする事が目的というよりも、GCC Developer Liteのみでコンパイルからプログラムの実行まで全て行うためだけに使う。
コンパイラオプションが正常に設定されていれば、「コンパイル(C)」メニュー内の「デバッグビルド&デバッガで開く(G)」が利用できるようになる。「デバッグビルド&デバッガで開く(G)」をクリックする(もしくは「Ctrl」+「F9」キーを押す)とコンパイラが呼び出され、問題が無ければソースコードから実行可能なプログラムが同じフォルダの中に生成される。
コンパイルが成功すると「デバッガ(GDB)を起動しますか?」と問い合わせてくるので、OKを押すとデバッガ(GDB)が開く。
GDBの操作説明は省くが、そのプロンプト上で「r」と入力してエンターキーを押すとコンパイルしたプログラムが実行される。
なおデバッガに興味を持ったのであれば、その使い方を検索してみると良い。
ソースコードの編集以外には大した機能は無いが、その中でも余り知られていない機能を紹介する。
紹介するソースコード上ではID番号が1~4に設定されたDYNAMIXELがあり、信速度は1Mbpsに変更してあるものとした。自身の環境向けにはPC上のDXHUBに割り当てられたCOMポートの番号を修正する程度で済む筈だ。ID番号が4つよりも少なくても多くても動かす事はできるが、少なくともID番号が1と2に設定されたDYNAMIXELが無いと一部の機能の確認ができない。
またDYNAMIXELのコントロールテーブルには機能を大きく左右するOperating Modeなるアイテムがあり、出荷時は3(Position Control Mode)に設定されている。紹介するソースコードによってはそのモードを変更しており、DYNAMIXELの電源を投入し直してもモードはそのまま保持される。他の目的でDYNAMIXELを使用する際はOperating Modeが変更されている可能性がある事を常に失念しないよう注意する事。
さらに初期条件を整える時を除いてエラー処理の類いを行っておらず、何かしらの不都合があったとしてもプログラムの実行は停止しないまでか異常終了する事も考えられる。DX2LIBのAPIの大半は戻り値で実行の正否の判定ができるので、堅牢なアプリケションを構成する際は判断材料にすると良いだろう。
少なくともソースコードをコンパイルするに当たっての前提は、
である。
なおGCC Developer LiteはGNUツールを用いてコンパイルしているため、MSYS2の環境でもコンパイルできる。またCOMポート名や時間にかかる関数を修正すれば、他のOSでも容易に流用できる。
以後のソースコードには、APIを使用するのに共通する最低限必要な処理が含まれている。
最終的には検索とCOMポートのクローズの間に実際に行いたい処理を追記する事になる。
DXL_ScanDevicesによる検索はDYNAMIXELのモデルが想定と違っていてもある程度順応できるようにと考えての事だが、諸設定が最適に設定されていたとしても検索が終わるまではそれ相応の時間(約10秒程度)かかる。
// DYNAMIXELの検索とモデル名の列挙 #include <stdio.h> #define _DYNAMICLOAD // DX2LIBの動的呼び出しを宣言 #include "dx2lib.h" // DX2LIBのヘッダ int main (void) { // DLLの読み込み(成功は必須) if (LoadDLL ()) { uint8_t IDs[253]; // 見つかったIDのリスト int detectnum = 0; // 見つかった数 // COMポートのオープン TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); // devが0でなければオープン成功 if (dev) { // 検索 detectnum = DXL_ScanDevices (dev, IDs); // 見つかったモデルをコンソールに列挙 DXL_PrintDevicesList ((void *)&printf); // この辺りに目的の処理を追記 // COMポートのクローズ DX2_ClosePort (dev); } // DLLの破棄 UnloadDLL (); } }
正常に検索が終了すると左からID番号・モデル名・モデル番号の順に次のような情報が表示される。
[ 1] 2XL430-W250 ($0442) [ 2] 2XL430-W250 ($0442) [ 3] 2XL430-W250 ($0442) [ 4] 2XL430-W250 ($0442)
なおID番号が1~4のDYNAMIXELが存在する前提でのDXL_GetModelInfo APIを使った登録方法も一応紹介しておく。こちらは指定されたID番号のみを対象に登録できるかを判断するので、全ID番号を対象とした検索を行った場合と異なりほぼ一瞬で処理が終了する。
// ID番号を決め打ちする場合 #include <stdio.h> #define _DYNAMICLOAD #include "dx2lib.h" int main (void) { // DLLの読み込み(成功は必須) if (LoadDLL ()) { // COMポートのオープン TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); // devが0でなければオープン成功 if (dev) { // 検索の代わりにID番号を直接指定して登録 DXL_GetModelInfo (dev, 1); DXL_GetModelInfo (dev, 2); DXL_GetModelInfo (dev, 3); DXL_GetModelInfo (dev, 4); // 登録されたモデルをコンソールに列挙 DXL_PrintDevicesList ((void *)&printf); // この辺りに目的の処理を追記 // COMポートのクローズ DX2_ClosePort (dev); } // DLLの破棄 UnloadDLL (); } }
サイレントに動作確認を行うには、DYNAMIXELに搭載されているLEDを使うと良いだろう。
ID番号を指定してLEDを明滅させるDXL_SetLED APIを用いて、個々のID番号を順に指定してLEDをON/OFFさせる。なおAPIの処理そのものは一瞬で終わってしまうので、目に見えるように10回のループ内のLEDのONとOFFの後にSleep命令で500msの中断を挿入した。
// LEDのON/OFF #include <stdio.h> #define _DYNAMICLOAD #include "dx2lib.h" int main (void) { if (LoadDLL ()) { uint8_t IDs[253]; // 見つかったIDのリスト int detectnum = 0; // 見つかった数 TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev) { detectnum = DXL_ScanDevices (dev, IDs); DXL_PrintDevicesList ((void *)&printf); // とりあえず10回繰り返す for (int i = 0; i < 10; i++) { // 全軸のLEDをONに設定 printf ("ON "); for (int j = 0; j < detectnum; j++) DXL_SetLED (dev, IDs[j], true); Sleep (500); // 全軸のLEDをOFFに設定 printf ("OFF "); for (int j = 0; j < detectnum; j++) DXL_SetLED (dev, IDs[j], false); Sleep (500); } DX2_ClosePort (dev); } UnloadDLL (); } }
なおDYNAMIXELに何らかのアラームが発生した事によりLEDが自動点滅している間は、LEDの制御ができない。
試運転の際にも行っている通りトルクのONという操作を予め行っておかないと、位置や速度を指令してもホーンは動かない。コントロールテーブル上のアイテム名が「Torque Enable」となっているためにトルクと称しているが、実際には設定された制御モードに従って制御を開始・停止する事を意味している。
DXL_SetTorqueEnablesEquival APIを用いて引数で指定されたID番号の一覧を用いて一斉にトルクのON/OFFを切り替える。
// トルクのON/OFF #include <stdio.h> #define _DYNAMICLOAD #include "dx2lib.h" int main (void) { if (LoadDLL ()) { uint8_t IDs[253]; // 見つかったIDのリスト int detectnum = 0; // 見つかった数 TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev) { detectnum = DXL_ScanDevices (dev, IDs); DXL_PrintDevicesList ((void *)&printf); // とりあえず意味も無く5回繰り返す for (int i = 0; i < 5; i++) { // 全軸のトルクをONに設定 printf ("ON "); DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, true); Sleep (1000); // 全軸のトルクをOFFに設定 printf ("OFF "); DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, false); Sleep (1000); } DX2_ClosePort (dev); } UnloadDLL (); } }
トルクがOFFの時にDYNAMIXELは脱力し、外力でホーンを動かす事ができる。
またコントロールテーブル中のアイテムの一部にはトルクがONの状態では変更できないものもあるため、APIによっては必要に応じてトルクのOFFを発行しているものがある。
DYNAMIXELの本分である位置決め制御を行う。
DXL_SetOperatingModesEquival APIを使用して全DYNAMIXELの動作モードを3:Position Control Modeに設定(このAPIは内部でトルクOFFを発行する)。全軸のトルクをON。コメントにあるように1秒かけて全軸の角度を45度へ移動→2秒かけて全軸の角度を-45度へ移動→1秒かけて全軸の角度を0度へ移動。最後に全軸のトルクをOFFにして終了する。foo関数内のDXL_SetGoalAnglesAndTime APIは軸数分の角度の配列と現在の角度からの移動時間を秒で指定するAPIだが、DYNAMIXELへの指令を終えると移動時間を待たずに即時復帰する。そのため直後にSleep関数で移動している間だけプログラムの進捗を意図的に停止させている。
// 角度指令 #include <stdio.h> #define _DYNAMICLOAD #include "dx2lib.h" // 角度指令と時間待ち関数 bool foo (TDeviceID dvid, const uint8_t *ids, const double *angles, int num, double sec) { bool result = DXL_SetGoalAnglesAndTime (dvid, ids, angles, num, sec); Sleep (sec * 1000); // 移動完了まで待機 return result; } int main (void) { if (LoadDLL ()) { uint8_t IDs[253]; // 見つかったIDのリスト int detectnum = 0; // 見つかった数 double angles[253]; // 角度の配列 TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev) { detectnum = DXL_ScanDevices (dev, IDs); DXL_PrintDevicesList ((void *)&printf); // 全軸を 3: Position Control に設定 DXL_SetOperatingModesEquival (dev, IDs, detectnum, 3); // 全軸のトルクをONに設定 DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, true); // 1秒かけて全軸の角度を45度へ移動 printf ("45deg "); for (int j = 0; j < detectnum; j++) angles[j] = 45.0; foo (dev, IDs, angles, detectnum, 1.0); // 2秒かけて全軸の角度を-45度へ移動 printf ("-45deg "); for (int j = 0; j < detectnum; j++) angles[j] = -45.0; foo (dev, IDs, angles, detectnum, 2.0); // 1秒かけて全軸の角度を0度へ移動 printf ("0deg "); for (int j = 0; j < detectnum; j++) angles[j] = 0.0; foo (dev, IDs, angles, detectnum, 1.0); // 全軸のトルクをOFFに設定 DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, false); DX2_ClosePort (dev); } UnloadDLL (); } }
角度や時間を変えて実際の挙動がどのようになるか試して欲しい。
こちらもDYNAMIXELの本分であるフィードバック値の取得を行う。
全軸のDYNAMIXELのトルクをOFFにして外力でホーンを動かせる状態にした後、DXL_GetPresentAngles APIを使用して複数軸の現在角度を取得し、ID番号と合わせて取得した角度を全軸分表示する。角度の取得と表示を何かしらのキー入力を行うまでループし続ける。
// 角度取得 #include <stdio.h> #include <conio.h> #define _DYNAMICLOAD #include "dx2lib.h" int main (void) { if (LoadDLL ()) { uint8_t IDs[253]; // 見つかったIDのリスト int detectnum = 0; // 見つかった数 double angles[253]; // 角度の配列 TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev) { detectnum = DXL_ScanDevices (dev, IDs); DXL_PrintDevicesList ((void *)&printf); // 全軸を 3: Position Control に設定 DXL_SetOperatingModesEquival (dev, IDs, detectnum, 3); DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, false); // キー入力があるまでループ while (!_kbhit ()) { // 全軸の現在角度を取得 DXL_GetPresentAngles (dev,IDs, angles, detectnum); // 取得した角度を表示 printf ("r"); for (int j = 0; j < detectnum; j++) printf("[%d]%6.1f ", IDs[j], angles[j]); } DX2_ClosePort (dev); } UnloadDLL (); } }
現在のDYNAMIXELのホーンの位置を取得して他のDYNAMIXELへの角度指令値として利用する。
ID番号が1のDYNAMIXELのトルクをOFFにして外力でホーンを動かせる状態にし、DXL_GetPresentAngle APIで現在のホーンの角度を取得する。その後DXL_SetGoalAngles APIを用いてその角度を全軸のDYNAMIXELへ指令する。ID番号1に対しても角度を指令しているが、予めトルクをOFFにしているのでホーンは動かない。角度の取得と位置の指令を何かしらのキー入力を行うまでループし続ける。
// 角度取得と角度指令 #include <stdio.h> #include <conio.h> #define _DYNAMICLOAD #include "dx2lib.h" int main (void) { if (LoadDLL ()) { uint8_t IDs[253]; // 見つかったIDのリスト int detectnum = 0; // 見つかった数 double angle = 0; // 現在の角度 double angles[253]; // 角度の配列 TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev) { detectnum = DXL_ScanDevices (dev, IDs); DXL_PrintDevicesList ((void *)&printf); // 全軸を 3: Position Control に設定 DXL_SetOperatingModesEquival (dev, IDs, detectnum, 3); DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, true); // ID=1のみトルクをOFFに設定 DXL_SetTorqueEnable (dev, 1, false); // キー入力があるまでループ while (!_kbhit ()) { // ID=1の現在角度を取得 DXL_GetPresentAngle (dev, 1, &angle); printf("rPresent Angle = %4.1f ", angle); // 全軸に同じ目標角度を指令 for (int j = 0; j < detectnum; j++) angles[j] = angle; DXL_SetGoalAngles (dev, IDs, angles, detectnum); } DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, false); DX2_ClosePort (dev); } UnloadDLL (); } }
なお前述のDXL_SetGoalAnglesAndTime APIを使ったプログラムを実行すると、移動時間の情報がDYNAMIXELに残ったままになる。その直後に本プログラムを実行した場合、ID番号1のホーンの動きに他のホーンが即応しないように見える。その場合はDYNAMIXELへ供給していた電源を一旦OFFにした後に再度ONにしてからプログラムを実行する事。
最高速度はあまり速くはないが、DYNAMIXELにホイールを装着して台車の足回りに使う事ができる。
DXL_SetOperatingModesEquival APIを使用して全DYNAMIXELの動作モードを1:Velocity Controlに設定(このAPIは内部でトルクOFFを発行する)。全軸のトルクをON。DXL_SetGoalVelocities APIを使用して全軸の角速度を30deg/sに設定→全軸の角速度を-30deg/sに設定→全軸の角速度を0deg/sに設定。最後に全軸のトルクをOFFにして終了する。
// 角速度指令 #include <stdio.h> #include <conio.h> #define _DYNAMICLOAD #include "dx2lib.h" int main (void) { if (LoadDLL ()) { uint8_t IDs[253]; // 見つかったIDのリスト int detectnum = 0; // 見つかった数 double velo[253]; // 角速度の配列 TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev) { detectnum = DXL_ScanDevices (dev, IDs); DXL_PrintDevicesList ((void *)&printf); // 全軸を 1: Velocity Control に設定 DXL_SetOperatingModesEquival (dev, IDs, detectnum, 1); DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, true); // 全軸の角速度を30deg/sに printf ("30deg/s "); for (int j = 0; j < detectnum; j++) velo[j] = 30.0; DXL_SetGoalVelocities (dev, IDs, velo, detectnum); Sleep (5000); // 5秒待ち // 全軸の角速度を-30deg/sに printf ("-30deg/s "); for (int j = 0; j < detectnum; j++) velo[j] = -30.0; DXL_SetGoalVelocities (dev, IDs, velo, detectnum); Sleep (5000); // 5秒待ち // 全軸の角速度を0deg/sに printf ("0deg/s "); for (int j = 0; j < detectnum; j++) velo[j] = 0.0; DXL_SetGoalVelocities (dev, IDs, velo, detectnum); Sleep (1000); // 1秒待ち DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, false); DX2_ClosePort (dev); } UnloadDLL (); } }
角度指令の応用で、各軸に与える角度指令値を時間に応じて複数パターンで変遷させる。
ここでは各DYNAMIXELの目標角度とその角度へ移行させる時間をTMotion構造体とし、複数のTMotion構造体を連続して実行させる事で一連の動きを実現している。またID番号1~8があるものとしてDXL_GetModelInfo APIを使用して検索時間を無くした。
GetQueryPerformanceCounter APIはPCの起動からの時間をmsで取得できるので、モーションで指定された時間を待機する際に利用している。
// モーション再生 #include <stdio.h> #include <conio.h> #define _DYNAMICLOAD #include "dx2lib.h" #define AXISNUM (8) // 軸数 #define MSize(d) (sizeof(d) / sizeof(d[0])) // 存在の有無にかかわらず指定軸数分のID番号のテーブル const uint8_t IDs[AXISNUM] = { 1, 2, 3, 4, 5, 6, 7, 8 }; // 各軸の角度と遷移時間の構造体 typedef struct { double angles[AXISNUM]; // 軸数分の角度情報 double sec; // 遷移時間 } TMotion; // モーションデータ1 // 8軸を1秒かけて0度(センター)に移動 const TMotion motion1[] = { {{ 0, 0, 0, 0, 0, 0, 0, 0}, 1.0}, }; // モーションデータ2 // 8軸の5つの様々な型を各秒数かけて移動 const TMotion motion2[] = { {{ 90, 390, 90, 90, 90, 90, 90, 90}, 3.0}, {{ 0, 0, 0, 0, 0, 0, 0, 0}, 2.0}, {{-290,-190, -90, -90, -90, -90, -90, -90}, 3.0}, {{ 45, 145, 45, 45, 45, 45, 45, 45}, 3.0}, {{ -30,-230, -30, -30, -30, -30, -30, -30}, 3.0}, }; // モーション再生 void PLAY (TDeviceID dev, const uint8_t *ids, const TMotion *motion, int framenum) { for (int i = 0; i < framenum; i++) { double t = GetQueryPerformanceCounter () + motion[i].sec * 1000; DXL_SetGoalAnglesAndTime (dev, ids, motion[i].angles, AXISNUM, motion[i].sec); while (t > GetQueryPerformanceCounter ()) Sleep (1); } } int main (void) { if (LoadDLL ()) { TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev != 0) { for (int i = 0; i < AXISNUM; i++) printf ("id=%2d, ModelName=%sn", IDs[i], DXL_GetModelInfo (dev, IDs[i])->name); // 全軸を 4: Expand Position Control に設定 DXL_SetOperatingModesEquival (dev, IDs, AXISNUM, 4); DXL_SetTorqueEnablesEquival (dev, IDs, AXISNUM, true); // motion1を再生 printf("motion1 "); PLAY (dev, IDs, motion1, MSize (motion1)); // motion2を再生 printf("motion2 "); PLAY (dev, IDs, motion2, MSize (motion2)); DXL_SetTorqueEnablesEquival (dev, IDs, AXISNUM, false); DX2_ClosePort (dev); } UnloadDLL (); } }
1つのパケット処理にかかる時間が短ければ単位時間当たりにこなせるパケット数が増えるため、自ずと通信速度が高い環境が望まれる。その恩恵に与かれるものとしてOSの並列処理が挙げられる。
ここでは角度と電流値を取得してモニタし続ける処理をthread1、角度を指令し続ける処理をthread2といった具合に個別のスレッドに割り当てた。下準備と終了処理はmain内で行っているが、それ以外は各々のスレッドが各々独立して処理している。
各々のスレッドに使用しているAPIが互いを意識しないコードになっているのは、DX2LIBのAPIが排他的に処理されるように作られているためである。しかし1つのスレッドでAPIを無限ループで遅滞なく呼び出し続けられてしまうとリソースを独占されてしまうため、ループ中にSleepを入れて適宜コンテキストスイッチを促している。
// マルチスレッド #include <stdio.h> #include <conio.h> #define _DYNAMICLOAD #include "dx2lib.h" #include <pthread.h> #include <math.h> bool term = false; // スレッド終了フラグ // スレッドに渡すパラメータ typedef struct { TDeviceID dev; // デバイスID const uint8_t *ids; // IDの配列 int num; // ID数 } TThInfo; // スレッド1 (モニタ) void *thread1 (TThInfo *info) { double pangle[info->num], pcur[info->num]; while (!term) { // 現在位置取得 if (DXL_GetPresentAngles (info->dev, info->ids, pangle, info->num)) { // 現在電流取得 if (DXL_GetPresentCurrents(info->dev, info->ids, pcur, info->num)) { printf("r"); for (int i = 0; i < info->num; i++) printf ("(%d:%5.1f %6.1f) ", info->ids[i], pangle[i], pcur[i]); } } Sleep (5); } return (void *)NULL; } // スレッド2 (正弦波で角度指令) void *thread2 (TThInfo *info) { double ang, angle[info->num]; while (!term) { // 時間から±60度の角度を生成 (4秒周期) ang = sin (0.5 * M_PI * GetQueryPerformanceCounter () * 0.001) * 60.0; for (int i = 0; i < info->num; i++) angle[i] = ang; DXL_SetGoalAngles (info->dev, info->ids, angle, info->num); Sleep (5); } return (void *)NULL; } int main (void) { pthread_t th1, th2; if (LoadDLL ()) { uint8_t IDs[253]; int detectnum = 0; TDeviceID dev = DX2_OpenPort ("\\.\COM3", 1000000); if (dev) { detectnum = DXL_ScanDevices (dev, IDs); DXL_PrintDevicesList ((void *)&printf); // 全軸を 3: Position Control に設定 DXL_SetOperatingModesEquival (dev, IDs, detectnum, 3); DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, true); // 2つのスレッドを作成 TThInfo info = { dev, IDs, detectnum }; pthread_create(&th1, NULL, (void *)thread1, (void *)&info); pthread_create(&th2, NULL, (void *)thread2, (void *)&info); // キー入力があるまでループ while (!_kbhit ()) { Sleep (100); } // スレッド終了を励起 term = true; // スレッド終了待ち pthread_join(th1, NULL); pthread_join(th2, NULL); DXL_SetTorqueEnablesEquival (dev, IDs, detectnum, false); DX2_ClosePort (dev); } UnloadDLL (); } }
同時に他のアプリケーションを使用すると、大抵の場合DYNAMIXELの挙動がおかしくなる。実際にはプログラムの実行が他のアプリケーションによって阻害され、
単純なループで高速に角度指令を行っている処理(こういった角度指令の仕方は旧来のDYNAMIXELの常套手段だった)が間延びしてしまう事が原因だ。マルチタスク環境において何がどのように影響するかを微調整する術はあるので、時間があるときにでも調べて欲しい。
またあえて諸々の通信速度を9600bpsに変更した場合も、最終的な動作にどの程度影響するかを試しても面白い。
その結果、Windows上でいわゆるリアルタイム性を担保するのは一筋縄ではいかない事が分かってもらえれば良いかと。
DX2LIBのページに紹介されている通りだが、その中でも流行のPythonで遊んでみても面白いかと思う。
GCC Developer LiteにはPythonが含まれており、コンパイルオプションで「Python(32bit)」や「Python(64bit)」を選ぶ事でライブラリに同梱されるPythonのサンプルソースコードが利用できる。しかし追加APIを利用したものばかりで、コントロールテーブルを読み書きする基本的なAPIは使われていない。
ここでは基本的なAPIのみを使った例を紹介するが、dx2lib.pyはC言語で作成されたライブラリをPythonで利用するためだけの記述がなされているため、Pythonならではの柔軟なコードがctypesによって変に厳格化されて使い勝手が悪い。
ひとまずDynamixel Xシリーズを前提とし、主要なアイテムのアドレスとコントロールテーブルを宣言してみた。
from ctypes import * ADDRESS_X_MODEL_NUMBER = 0 #W ADDRESS_X_MODEL_INFORMATION = 4 #L ADDRESS_X_VERSION_FW = 6 #B ADDRESS_X_ID = 7 #B ADDRESS_X_BAUDRATE = 8 #B ADDRESS_X_RETURN_DELAY_TIME = 9 #B ADDRESS_X_DRIVE_MODE = 10 #B ADDRESS_X_OPERATIING_MODE = 11 #B ADDRESS_X_HOMING_OFFSET = 20 #L ADDRESS_X_MAX_POSITION_LIMIT = 48 #L ADDRESS_X_MIN_POSITION_LIMIT = 52 #L ADDRESS_X_TORQUE_ENABLE = 64 #B ADDRESS_X_LED_RED = 65 #B ADDRESS_X_STATUS_RETURN_LEVEL = 69 #B ADDRESS_X_HARDWARE_ERROR_STATUS = 70 #B ADDRESS_X_VELOCITY_IGAIN = 76 #W ADDRESS_X_VELOCITY_PGAIN = 78 #W ADDRESS_X_POSITION_DGAIN = 80 #W ADDRESS_X_POSITION_IGAIN = 82 #W ADDRESS_X_POSITION_PGAIN = 84 #W ADDRESS_X_GOAL_PWM = 100 #W ADDRESS_X_GOAL_CURRENT = 102 #W ADDRESS_X_GOAL_VELOCITY = 104 #L ADDRESS_X_PROF_ACCELERATION = 108 #L ADDRESS_X_PROF_VELOCITY = 112 #L ADDRESS_X_GOAL_POSITION = 116 #L ADDRESS_X_MOVING = 122 #B ADDRESS_X_MOVING_STATUS = 123 #B ADDRESS_X_PRESENT_PWM = 124 #W ADDRESS_X_PRESENT_CURRENT = 126 #W ADDRESS_X_PRESENT_VELOCITY = 128 #L ADDRESS_X_PRESENT_POSITION = 132 #L ADDRESS_X_VELOCITY_TRAJECTORY = 136 #L ADDRESS_X_POSITION_TRAJECTORY = 140 #L ADDRESS_X_PRESENT_VOLTAGE = 144 #W ADDRESS_X_PRESENT_TEMP = 146 #B class XCtrlTable(Structure): _pack_ = 1 _fields_ = [ ('ModelNumber', c_uint16), ('ModelInformation', c_uint32), ('VersionofFirmware', c_uint8), ('ID', c_uint8), ('Baudrate', c_uint8), ('ReturnDelayTime', c_uint8), ('DriveMode', c_uint8), ('OperatingMode', c_uint8), ('SecondaryID', c_uint8), ('ProtocolVersion', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('HomingOffset', c_int32), ('MovingThreshold', c_uint32), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('TemperatureLimit', c_uint8), ('MaxVoltageLimit', c_uint16), ('MinVoltageLimit', c_uint16), ('PWMLimit', c_uint16), ('CurrentLimit', c_uint16), ('AccelerationLimit', c_uint32), ('VelocityLimit', c_uint32), ('MaxPositionLimit', c_uint32), ('MinPositionLimit', c_uint32), ('ExternalPortMode1', c_uint8), ('ExternalPortMode2', c_uint8), ('ExternalPortMode3', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('Shutdown', c_uint8), ('TorqueEnable', c_uint8), ('LED', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('StatusReturnLevel', c_uint8), ('RegisteredInstruction', c_uint8), ('HardwareErrorStatus', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('VelocityIGain', c_uint16), ('VelocityPGain', c_uint16), ('PositionDGain', c_uint16), ('PositionIGain', c_uint16), ('PositionPGain', c_uint16), ('reserve', c_uint8), ('reserve', c_uint8), ('FeedforwardAccelerationGain', c_uint16), ('FeedforwardVelocityGain, ', c_uint16), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('BusWatchdog', c_int8), ('reserve', c_uint8), ('GoalPWM', c_int16), ('GoalCurrent', c_int16), ('GoalVelocity', c_int32), ('ProfileAcceleration', c_uint32), ('ProfileVelocity', c_uint32), ('GoalPosition', c_int32), ('RealtimeTick', c_uint16), ('Moving', c_uint8), ('MovingStatus', c_uint8), ('PresentPWM', c_int16), ('PresentCurrent', c_int16), ('PresentVelocity', c_int32), ('PresentPosition', c_int32), ('VelocityTrajectory', c_uint32), ('PositionTrajectory', c_uint32), ('PresentInputVoltage', c_uint16), ('PresentTemperature', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('reserve', c_uint8), ('ExternalPortData1', c_uint16), ('ExternalPortData2', c_uint16), ('ExternalPortData3', c_uint16)] class VelocityGain(Structure): _pack_ = 1 _fields_ = [ ('VelocityIGain', c_uint16), ('VelocityPGain', c_uint16)] class PositionGain(Structure): _pack_ = 1 _fields_ = [ ('PositionDGain', c_uint16), ('PositionIGain', c_uint16), ('PositionPGain', c_uint16)]
例えばPresent PositionをID=1のDynamixelのコントロールテーブルから読み出すには、以下のコードで対応できる。
from dx2lib import * dev = DX2_OpenPort (b'\\.\COM3', 57600) if dev != None: ppos = c_int32() err = c_uint16() DX2_ReadLongData (dev, 1, ADDRESS_X_PRESENT_POSITION, cast(byref(ppos), POINTER(c_uint32)), err) DX2_ClosePort (dev)
更にXCtrlTable構造体を用いて変数を定義し、その変数へID=1のDynamixelのコントロールテーブルから一括読み出した値を保存する場合はDX2_ReadBlockDataを用いる。
from dx2lib import * dev = DX2_OpenPort (b'\\.\COM3', 57600) if dev != None: tbl = XCtrlTable() err = c_uint16() if DX2_ReadBlockData (dev, 1, ADDRESS_X_MODEL_NUMBER, cast(byref(tbl), POINTER(c_uint8)), sizeof(tbl), err): for field in tbl._fields_: if field[0] != 'reserve': print (tbl.__class__.__dict__[field[0]].offset, field[0], '=', getattr(tbl, field[0])) DX2_ClosePort (dev)
位置決め制御時のPIDゲインを設定する等の連続したアドレスに割り当てられた複数のアイテムを同時に書き換える場合はDX2_WriteBlockDataを用いる。
from dx2lib import * dev = DX2_OpenPort (b'\\.\COM3', 57600) if dev != None: gain = PositionGain(0, 0, 400) DX2_WriteBlockData (dev, 1, ADDRESS_X_POSITION_DGAIN, cast(byref(gain), POINTER(c_uint8)), sizeof (gain), None) DX2_ClosePort (dev)
追加APIに無い機能は基本APIを使ってコントロールテーブルへ直接アクセスするしかないため、参考にしてもらえればと思う。
全てPythonで書いてしまえば、態々Cで作られたライブラリをリンクしたり、ctypesの有象無象に呵まれる事も無くスッキリする。という事でインストラクションパケットの生成と送信、ステータスパケットの受信、インストラクションの種類によってパラメータ部分を結合・分解するクラスを作ってみた。前記の内容とは一線を画すので、互換性は一切考えていない。
pySerialは必須。
なお単体で試せるようにデモコードを付けておいた。ターゲットはXM/XH/XDシリーズ、IDは1~5(少なくともID=1は必須)、ボーレートは3Mbpsを想定、デバッグ用にpsutilが必要、マルチスレッドでアクセスといったところだ。プロトコルV1に対応したDynamixelの場合は末尾のコメントになっているコードを利用してもらえればと。
一応関数に与えられたパラメータを確認しているが、全領域で検証したものではない。また値の正負については勝手に判断している部分もあるので、都合が悪ければ適宜修正してもらえればと思う。
#!/usr/bin/python3 # -*- coding: utf-8 -*- # # pyDXL.py # (C) 2024 BestTechnology import serial, threading, array, struct from collections import namedtuple from struct import pack, unpack, iter_unpack ########################################################## # Functionalized the part of converting int to bytes. # If specified as a tuple, it is converted to bytes at once. ########################################################## def B2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return bytes(((d & 0x7f) | 0x80) if d < 0 else d for d in d) else: return bytes(pack('<B', ((d & 0x7f) | 0x80) if d < 0 else d)) def W2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<H',d) for d in [((d & 0x7fff) | 0x8000) if d < 0 else d for d in d]]) else: return bytes(pack('<H', ((d & 0x7fff) | 0x8000) if d < 0 else d)) def L2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<I',d) for d in [((d & 0x7fffffff) | 0x80000000) if d < 0 else d for d in d]]) else: return bytes(pack('<I', ((d & 0x7fffffff) | 0x80000000) if d < 0 else d)) ########################################################## # API for Dynamixel protocol V1 ########################################################## class DXLProtocolV1: BROADCASTING_ID = 0xfe INST_PING = 0x01 INST_READ = 0x02 INST_WRITE = 0x03 INST_REG_WRITE = 0x04 INST_ACTION = 0x05 INST_FACTORY_RESET = 0x06 INST_REBOOT = 0x08 INST_SYNC_WRITE = 0x83 INST_SYNG_REG_WRITE = 0x85 TSyncW = namedtuple("TSyncW", ("id", ("data"))) def __init__(self, port : serial.Serial | str, baudrate = 57600, timeout = 0.05, mutex = None): """ Initalize parameters ------------- port : serial.Serial | str pySerial or Device name baudrate : int Serial baudrate[bps] timeout : float Read timeout[s] """ if isinstance(port, serial.Serial): self.__serial = port self.__baudrate = port.baudrate self.__timeout = port.timeout else: print(port, baudrate, timeout) print(type(port)) self.__serial = serial.Serial(port, baudrate = baudrate, timeout = timeout) self.__baudrate = self.__serial.baudrate self.__timeout = self.__serial.timeout if mutex == None: self.__mutex = threading.Lock() else: self.__mutex = mutex self.__Error = 0 @property def mutex(self): return self.__mutex @property def baudrate(self): return self.__serial.baudrate @baudrate.setter def baudrate(self, baudrate): self.__baudrate = baudrate self.__serial.baudrate = baudrate @property def timeout(self): return self.__serial.timeout @timeout.setter def timeout(self, timeout): self.__timeout = timeout self.__serial.timeout = timeout def __reconfig(self): self.__serial.baudrate = self.__baudrate self.__serial.timeout = self.__timeout @property def Error(self): return self.__Error def TxPacket(self, id : int, inst : int, param : bytes, echo = False) -> (bytes, bool): """ Sending packets parameters ------------- id : int Target ID inst : int Instruction command param : bytes Packet parameters Returns ------- bytes Packets sent bool Success or failure """ self.__reconfig() if ((id == self.BROADCASTING_ID) or (id >= 0 and id <= 253)) and len(param) <= (256 - 6): instp = bytearray([0xff,0xff,id,0,inst]) + bytes(param) instp[3] = len(instp) - 3 instp += B2Bs(~sum(instp[2:]) & 0xff) self.__serial.reset_input_buffer() if echo: print('TX:', instp.hex(':')) self.__serial.write(instp) return bytes(instp), True return None, False def __rx(self, length) -> bytes: s = self.__serial.read(length) l = len(s) if l == length: return s else: r = s length -= l while self.__serial.in_waiting > 0: s = self.__serial.read(length) r += s length -= len(s) if length == 0: break return r def RxPacket(self, echo = False) -> (bytes, bool): """ Receiving packets Returns ------- bytes Packets received bool Success or failure """ statp = self.__rx(5) if statp: if len(statp) == 5: if statp[0] == 0xff and statp[1] == 0xff: l = statp[3] - 1 self.__Error = statp[4] statp += self.__rx(l) if len(statp) == l + 5: if statp[-1:][0] == ((~sum(statp[2:-1])) & 0xff): if echo: print('RX:', statp.hex(':')) return bytes(statp), (statp[4] & 0x40) == 0 return None, False def Write(self, id : int, addr : int, data : bytes, echo = False) -> bool: """ Write instruction parameters ------------- id : int Target ID addr : int Target item address data : bytes Data to be written Returns ------- result : bool Success or failure """ with self.__mutex: if id >= 0 and id <= self.BROADCASTING_ID and addr >= 0 and addr <= 254: if self.TxPacket(id, self.INST_WRITE, B2Bs(addr) + data, echo)[1]: if id != self.BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return dat[2] == id and (dat[4] & 0x18) == 0 else: return True return False def Write8(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, B2Bs(data), echo) def Write16(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, W2Bs(data), echo) def Write32(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, L2Bs(data), echo) def Read(self, id : int, addr : int, length : int, echo = False) -> bytes: """ Read instruction parameters ------------- id : int Target ID addr : int Target item address length : int Number of bytes to read Returns ------- bytes Data read """ with self.__mutex: if id >= 0 and id <= 253 and addr >= 0 and addr <= 254 and length > 0 and length <= (256 - 6): if self.TxPacket(id, self.INST_READ, B2Bs((addr, length)), echo)[1]: dat, r = self.RxPacket(echo) if r: if dat[2] == id and (dat[4] & 0x8) == 0: return bytes(dat[5:-1]) return None def Read8(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, length, echo) if r != None: n = sum(iter_unpack('b' if signed else 'B', r), ()) return n if length > 1 else n[0] return None def Read16(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 2 << (length - 1), echo) if r != None: n = sum(iter_unpack('h' if signed else 'H', r), ()) return n if length > 1 else n[0] return None def Read32(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 4 << (length - 1), echo) if r != None: n = sum(iter_unpack('i' if signed else 'I', r), ()) return n if length > 1 else n[0] return None def SyncWrite(self, addr : int, length : int, id_datas : (TSyncW), echo = False) -> bool: """ Sync Write instruction parameters ------------- addr : int Target item address length : int Number of bytes to write id_datas : (TSyncW) Target ID and data Returns ------- bool Success or failure """ with self.__mutex: if addr >= 0 and addr <= 254 and length > 0 and length < (256 - 6): param = B2Bs((addr,length)) for d in id_datas: param += B2Bs(d.id) + d.data if len(d.data) != length or d.id < 0 or d.id > 253: del param return False return self.TxPacket(self.BROADCASTING_ID, self.INST_SYNC_WRITE, param, echo)[1] return False def Ping(self, id : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.INST_PING, bytes(), echo)[1]: dat, r = self.RxPacket(echo) if r: return id == dat[2] and dat[3] == 2 return False def FactoryReset(self, id : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.INST_FACTORY_RESET, bytes(), echo)[1]: dat, r = self.RxPacket(echo) if r: return id == dat[2] and dat[3] == 2 return False def Reboot(self, id : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.INST_REBOOT, bytes(), echo)[1]: dat, r = self.RxPacket(echo) if r: return id == dat[2] and dat[3] == 2 return False ########################################################## # API for Dynamixel protocol V2 ########################################################## class DXLProtocolV2: BROADCASTING_ID = 0xfe INST_PING = 0x01 INST_READ = 0x02 INST_WRITE = 0x03 INST_REG_WRITE = 0x04 INST_ACTION = 0x05 INST_FACTORY_RESET = 0x06 INST_REBOOT = 0x08 INST_SYS_WRITE = 0x0d INST_CLEAR = 0x10 INST_CONTROL_TABLE_BACKUP = 0x20 INST_STATUS = 0x55 INST_SYNC_READ = 0x82 INST_SYNC_WRITE = 0x83 INST_SYNG_REG_WRITE = 0x85 INST_FAST_SYNC_READ = 0x8a INST_BULK_READ = 0x92 INST_BULK_WRITE = 0x93 INST_FAST_BULK_READ = 0x9a TSyncW = namedtuple("TSyncW", ("id", ("data"))) TBulkW = namedtuple("TBulkW", ("id", "addr", ("data"))) TBulkR = namedtuple("TBulkR", ("id", "addr", "length")) __crc16_lutable = array.array('H') def __init__(self, port : serial.Serial | str, baudrate = 57600, timeout = 0.05, mutex = None): """ Initalize parameters ------------- port : serial.Serial | str pySerial or Device name baudrate : int Serial baudrate[bps] timeout : float Read timeout[s] """ if isinstance(port, serial.Serial): self.__serial = port self.__baudrate = port.baudrate self.__timeout = port.timeout else: print(port, baudrate, timeout) print(type(port)) self.__serial = serial.Serial(port, baudrate = baudrate, timeout = timeout) self.__baudrate = self.__serial.baudrate self.__timeout = self.__serial.timeout if mutex == None: self.__mutex = threading.Lock() else: self.__mutex = mutex self.__Error = 0 poly = 0x8005 for i in range(256): nData = i << 8 nAccum = 0 for j in range(8): nAccum = ((nAccum << 1) ^ poly if (nData ^ nAccum) & 0x8000 else nAccum << 1) & 0xffff nData <<= 1 self.__crc16_lutable.append(nAccum) @property def mutex(self): return self.__mutex @property def baudrate(self): return self.__serial.baudrate @baudrate.setter def baudrate(self, baudrate): self.__baudrate = baudrate self.__serial.baudrate = baudrate @property def timeout(self): return self.__serial.timeout @timeout.setter def timeout(self, timeout): self.__timeout = timeout self.__serial.timeout = timeout def __reconfig(self): self.__serial.baudrate = self.__baudrate self.__serial.timeout = self.__timeout @property def Error(self): return self.__Error def __crc16(self, data : bytes) -> int: crc = 0 for d in data: crc = (crc << 8) ^ self.__crc16_lutable[(((crc >> 8) ^ d) & 0xff)] return crc & 0xffff def TxPacket(self, id : int, inst : int, param : bytes, echo = False) -> (bytes, bool): """ Sending packets parameters ------------- id : int Target ID inst : int Instruction command param : bytes Packet parameters Returns ------- bytes Packets sent bool Success or failure """ self.__reconfig() if ((id == self.BROADCASTING_ID) or (id >= 0 and id <= 252)) and len(param) < (65536 - 10): instp = bytearray([0xff,0xff,0xfd,0x00,id,0,0,inst]) + bytearray(param).replace(b'xffxffxfd',b'xffxffxfdxfd') instp[5:7] = W2Bs(len(instp) - 5) instp += W2Bs(self.__crc16(instp)) self.__serial.reset_input_buffer() if echo: print('TX:', instp.hex(':')) self.__serial.write(instp) return bytes(instp), True return None, False def __rx(self, length) -> bytes: s = self.__serial.read(length) l = len(s) if l == length: return s else: r = s length -= l while self.__serial.in_waiting > 0: s = self.__serial.read(length) r += s length -= len(s) if length == 0: break return r def RxPacket(self, echo = False) -> (bytes, bool): """ Receiving packets Returns ------- bytes Packets received bool Success or failure """ self.__serial.flush() statp = self.__rx(9) if statp: if len(statp) == 9: if statp[0] == 0xff and statp[1] == 0xff and statp[2] == 0xfd and statp[3] == 0 and statp[7] == 0x55: l = unpack('<H', statp[5:7])[0] - 2 self.__Error = statp[8] statp += self.__rx(l) if len(statp) == l + 9: if unpack('<H', statp[-2:])[0] == self.__crc16(statp[:-2]): statp = statp[0:9] + statp[9:].replace(b'xffxffxfdxfd',b'xffxffxfd') if echo: print('RX:', statp.hex(':')) return bytes(statp), (statp[8] & 0x7f) == 0 return None, False def Write(self, id : int, addr : int, data : bytes, echo = False) -> bool: """ Write instruction parameters ------------- id : int Target ID addr : int Target item address data : bytes Data to be written Returns ------- result : bool Success or failure """ with self.__mutex: if ((id >= 0 and id <= 252) or (id == self.BROADCASTING_ID)) and addr >= 0 and addr <= 65535: if self.TxPacket(id, self.INST_WRITE, W2Bs(addr) + data, echo)[1]: if id != self.BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return dat[4] == id and (dat[8] & 0x7f) == 0 else: return True return False def Write8(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, B2Bs(data), echo) def Write16(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, W2Bs(data), echo) def Write32(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, L2Bs(data), echo) def Read(self, id : int, addr : int, length : int, echo = False) -> bytes: """ Read instruction parameters ------------- id : int Target ID addr : int Target item address length : int Number of bytes to read Returns ------- bytes Data read bool Success or failure """ with self.__mutex: if id >= 0 and id <= 252 and addr >= 0 and addr <= 65535 and length > 0 and length < (65536 - 10): if self.TxPacket(id, self.INST_READ, W2Bs(addr) + W2Bs(length), echo)[1]: dat, r = self.RxPacket(echo) if r: return bytes(dat[9:-2]) return None def Read8(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, length, echo) if r != None: n = sum(iter_unpack('b' if signed else 'B', r), ()) return n if length > 1 else n[0] return None def Read16(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 2 << (length - 1), echo) if r != None: n = sum(iter_unpack('h' if signed else 'H', r), ()) return n if length > 1 else n[0] return None def Read32(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 4 << (length - 1), echo) if r != None: n = sum(iter_unpack('i' if signed else 'I', r), ()) return n if length > 1 else n[0] return None def SyncWrite(self, addr : int, length : int, id_datas : (TSyncW), echo = False) -> bool: """ Sync Write instruction parameters ------------- addr : int Target item address length : int Number of bytes to write id_datas : (TSyncW) Target ID and data Returns ------- bool Success or failure """ with self.__mutex: if addr >= 0 and addr <= 65535 and length > 0 and length < (65536 - 10): param = W2Bs(addr) + W2Bs(length) for d in id_datas: param += bytes((d.id,)) + d.data if len(d.data) != length or d.id < 0 or d.id > 252: del param return False return self.TxPacket(self.BROADCASTING_ID, self.INST_SYNC_WRITE, param, echo)[1] return False def SyncRead(self, addr : int, length : int, ids : (int), echo = False) -> tuple: """ Sync Read instruction parameters ------------- addr : int Target item address length : int Number of bytes to read ids : (int) Target IDs Returns ------- tuple Read ID and data """ with self.__mutex: result = () if addr >= 0 and addr < 65535 and length > 0 and length < (65536 - 10): if self.TxPacket(self.BROADCASTING_ID, self.INST_SYNC_READ, W2Bs(addr) + W2Bs(length) + B2Bs(ids), echo)[1]: for id in ids: dat, r = self.RxPacket(echo) if r: if dat[4] == id: result += (id, bytes(dat[9:9 + length])), else: result += (id, bytes([])), else: result += (id, bytes([])), return result def BulkWrite(self, data : (TBulkW), echo = False) -> bool: """ Bulk Write instruction parameters ------------- data : (TBulkW) Target ID and address, data ids : (TSyncW) Target ID and data Returns ------- bool Success or failure """ with self.__mutex: param = bytes() for d in data: if d.id >= 0 and d.id <= 252 and d.addr >= 0 and d.addr <= 65535: param += B2Bs(d.id) + W2Bs(d.addr) + W2Bs(len(d.data)) + d.data else: del param return False return self.TxPacket(self.BROADCASTING_ID, self.INST_BULK_WRITE, param, echo)[1] def BulkRead(self, data:(TBulkR), echo = False) -> tuple: """ Bulk Read instruction parameters ------------- data : (TBulkR) ID, address, and number of bytes to be read Returns ------- tuple Read ID and data """ with self.__mutex: result = () param = bytes() for d in data: if d.id < 0 or d.addr < 0 or d.length < 0: return result param += B2Bs(d.id) + W2Bs(d.addr) + W2Bs(d.length) if self.TxPacket(self.BROADCASTING_ID, self.INST_BULK_READ, param, echo)[1]: for d in data: rxd, r = self.RxPacket(echo) if r: if(d.id == rxd[4]): result += (d.id, bytes(rxd[9:-2])), else: result += (d.id, bytes([])), else: result += (d.id, bytes([])), del param return result def Ping(self, id : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.INST_PING, bytes(), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 7 and rxd[6] == 0 return False def FactoryReset(self, id : int, p1 : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.INST_FACTORY_RESET, bytes((p1,)), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 4 and rxd[6] == 0 return False def Reboot(self, id : int, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.INST_REBOOT, bytes(), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 4 and rxd[6] == 0 return False def Clear(self, id : int, val, echo = False) -> bool: with self.__mutex: if self.TxPacket(id, self.INST_CLEAR, B2Bs(1) + L2Bs(val), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 4 and rxd[6] == 0 return False ########################################################## # test code ########################################################## if __name__ == "__main__": from contextlib import contextmanager from threading import Thread import time, gc, psutil, os ec = False ID = 1 fin = False @contextmanager def stopwatch(): start_time = time.time() yield #print('..proc time={:.1f}ms {}'.format((time.time() - start_time) * 1000, psutil.Process(os.getpid()).memory_info().rss)) def func1(dx): global ID, fin, ec try: ''' """ reset dxl """ with stopwatch(): r = dx.FactoryReset(ID, 0xff, echo = ec) print(f'FactoryReset({ID}): {r}') time.sleep(0.5) ''' """ ping dxl """ print(f'Ping({ID})=', dx.Ping(ID, echo = ec)) """ reboot dxl """ with stopwatch(): r = dx.Reboot(ID, echo = ec) print(f'Reboot({ID})={r}') time.sleep(0.5) """ basic packet proc """ with stopwatch(): with dx.mutex: r0 = dx.TxPacket(ID, dx.INST_WRITE, (65, 0, 1)) r1 = dx.RxPacket() print(f'TxPacket({ID})={r0[1]}', r0[0].hex(':')) if r1[0]: print(f'RxPacket({ID})={r1[1]}', r1[0].hex(':')) """ dump memory """ l = 50 for addr in range(0, 700, l): r = dx.Read(ID, addr, l, echo = ec) print(f'Read({addr};{l})=',r.hex(':') if r else '!!!!!!!!!!!!!!!!!!!!!!!!') """ read byte item """ with stopwatch(): r = dx.Read8(ID, 65, echo = ec) print(f'Read8({ID})={r}') """ sync read inst. """ print('SyncRead=') with stopwatch(): d = dx.SyncRead(0, 4, (1, 2, 3, 4, 5), echo = ec) for d in d: print(f' ({d[0]}) 0,4 hex=', d[1].hex(':') if len(d[1]) > 0 else ()) with stopwatch(): d = dx.SyncRead(132, 4, (1, 2, 3, 4, 5), echo = ec) for d in d: print(f' ({d[0]}) 132,4 int32=', unpack('<i', pack('<I', unpack('<I', d[1])[0]))[0] if len(d[1]) > 0 else ()) """ sync write inst. """ for i in range(30): with stopwatch(): dx.SyncWrite(65, 1, (dx.TSyncW(1, B2Bs(1)), dx.TSyncW(2, B2Bs(1)), dx.TSyncW(3, B2Bs(1)), dx.TSyncW(4, B2Bs(1)), dx.TSyncW(5, B2Bs(1))), echo = ec) time.sleep(0.05) with stopwatch(): dx.SyncWrite(65, 1, (dx.TSyncW(1, B2Bs(0)), dx.TSyncW(2, B2Bs(0)), dx.TSyncW(3, B2Bs(0)), dx.TSyncW(4, B2Bs(0)), dx.TSyncW(5, B2Bs(0))), echo = ec) time.sleep(0.05) """ set goal position """ #torque off if dx.Write8(ID, 64, 0, echo = ec): #multi turn if dx.Write8(ID, 11, 4, echo = ec): #torque on if dx.Write8(ID, 64, 1, echo = ec): print(f'Write32/Read32({ID})') for gp in tuple(range(2047, 2047 + 4096, 64)) + tuple(range(2047 + 4096, 2047 - 4096, -64)) + tuple(range(2047 - 4096, 2047, 64)): for i in range(10): #set goal position with stopwatch(): dx.Write32(ID, 116, gp, echo = ec) #get present position with stopwatch(): pp = dx.Read32(ID, 132, signed = True, echo = ec) if pp != None: print(f' w={gp:6} r={pp:6} diff={gp-pp:5}', end='r') else: pass print('None ', end='r') break time.sleep(0.01) else: continue pass break print('') #torque off with stopwatch(): dx.Write8(ID, 64, 0, echo = ec) #normal turn with stopwatch(): dx.Write8(ID, 11, 3, echo = ec) """ block read/write (add/remove suffixes) """ with stopwatch(): r = dx.Read(ID, 120, 27, echo = ec) if r: print(f'Read({ID};128)=', tuple(iter_unpack('<HBBhhiiIIHB', r))[0]) fffffd = bytes(( 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, )) with stopwatch(): r = dx.Write(ID, 634, fffffd, echo = ec) print(f'Write({ID})={r}') with stopwatch(): r = dx.Read(ID, 634, len(fffffd), echo = ec) if r: print(f'Read({ID})=', r.hex(':')) """ bulk read inst. """ with stopwatch(): d = dx.BulkRead((dx.TBulkR(1,0,10), dx.TBulkR(2,0,20), dx.TBulkR(3,0,30), dx.TBulkR(4,0,40), dx.TBulkR(5,0,40)), echo = ec) print('BulkRead=') for d in d: print(f' ({d[0]},0)', d[1].hex(':')) dx.Write8(ID, 64, 1, echo = ec) """ bulk write inst. """ print('BulkWrite=') with stopwatch(): print(' 1', dx.BulkWrite((dx.TBulkW(1,104, L2Bs((0,0,0,1024))), dx.TBulkW(2,65,B2Bs(0)), dx.TBulkW(3,65,B2Bs(0)), dx.TBulkW(4,65,B2Bs(0))), echo = ec)) time.sleep(0.5) with stopwatch(): print(' 2', dx.BulkWrite((dx.TBulkW(1,104, L2Bs(0)+L2Bs(0)+L2Bs(0)+L2Bs(2048)), dx.TBulkW(2,65,B2Bs(1)), dx.TBulkW(3,65,B2Bs(1)), dx.TBulkW(4,65,B2Bs(1))), echo = ec)) time.sleep(0.5) with stopwatch(): print(' 3', dx.BulkWrite((dx.TBulkW(1,104, L2Bs(0)+L2Bs(0)+L2Bs(0)+L2Bs(1024)), dx.TBulkW(2,65,B2Bs(0)), dx.TBulkW(3,65,B2Bs(0))), echo = ec)) time.sleep(0.5) with stopwatch(): print(' 4', dx.BulkWrite((dx.TBulkW(1,116, L2Bs(2048)), dx.TBulkW(2,65,B2Bs(1)), dx.TBulkW(3,65,B2Bs(1))), echo = ec)) time.sleep(0.5) with stopwatch(): print(' 5', dx.BulkWrite((dx.TBulkW(1,65,bytes(0)),dx.TBulkW(2,65,bytes(0)),dx.TBulkW(3,65,bytes(0))), echo = ec)) dx.Write8(ID, 64, 0, echo = ec) except Exception as ex: pass import traceback print('--- Caught Exception ---') traceback.print_exc() print('------------------------') ''' trace = [] tb = ex.__traceback__ while tb is not None: trace.append({ "filename": tb.tb_frame.f_code.co_filename, "name": tb.tb_frame.f_code.co_name, "lineno": tb.tb_lineno }) tb = tb.tb_next print(str({ 'type': type(ex).__name__, 'message': str(ex), 'trace': trace })) ''' fin = True def func2(dx): global ID, fin, ec while not fin: try: l = dx.Read8(ID, 65, echo = ec) if l != None: l ^= 1 dx.Write8(ID, 65, l, echo = ec) time.sleep(0.05) except Exception as ex: import traceback print('--- Caught Exception ---') traceback.print_exc() print('------------------------') return def func3(dx): global ID, fin, ec try: """ dump memory """ l = 20 for addr in range(0, 250, l): r = dx.Read(ID, addr, l, echo = ec) print(f'Read({addr};{l})=',r.hex(':') if r else '!!!!!!!!!!!!!!!!!!!!!!!!') ''' """ reset dxl """ with stopwatch(): r = dx.FactoryReset(ID, echo = ec) print(f'FactoryReset({ID}): {r}') time.sleep(0.5) ''' """ ping dxl """ print(f'Ping({ID})=', dx.Ping(ID, echo = ec)) """ reboot dxl """ with stopwatch(): r = dx.Reboot(ID, echo = ec) print(f'Reboot({ID})={r}') time.sleep(1) """ basic packet proc """ with stopwatch(): with dx.mutex: r0 = dx.TxPacket(ID, dx.INST_WRITE, (25, 1)) r1 = dx.RxPacket() print(f'TxPacket({ID})={r0[1]}', r0[0].hex(':')) if r1[0]: print(f'RxPacket({ID})={r1[1]}', r1[0].hex(':')) """ read byte item """ with stopwatch(): r = dx.Read8(ID, 25, echo = ec) print(f'Read8({ID})={r}') """ sync write inst. """ for i in range(30): with stopwatch(): dx.SyncWrite(25, 1, (dx.TSyncW(1, B2Bs(1)), dx.TSyncW(2, B2Bs(1)), dx.TSyncW(3, B2Bs(1))), echo = ec) time.sleep(0.05) with stopwatch(): dx.SyncWrite(25, 1, (dx.TSyncW(1, B2Bs(0)), dx.TSyncW(2, B2Bs(0)), dx.TSyncW(3, B2Bs(0))), echo = ec) time.sleep(0.05) """ set goal position """ #torque off if dx.Write8(ID, 24, 0, echo = ec): #multi turn if dx.Write16(ID, 6, (4095, 4095), echo = ec): #torque on if dx.Write8(ID, 24, 1, echo = ec): print(f'Write16/Read16({ID})') for gp in tuple(range(2047, 2047 + 4096, 64)) + tuple(range(2047 + 4096, 2047 - 4096, -64)) + tuple(range(2047 - 4096, 2047, 64)): for i in range(10): #set goal position with stopwatch(): dx.Write16(ID, 30, gp, echo = ec) #get present position with stopwatch(): pp = dx.Read16(ID, 36, signed = True, echo = ec) if pp != None: print(f' w={gp:6} r={pp:6} diff={gp-pp:5}', end='r') else: pass print('None ', end='r') break time.sleep(0.005) else: continue pass break print('') #torque off dx.Write8(ID, 24, 0, echo = ec) #normal turn with stopwatch(): dx.Write16(ID, 6, (0, 4095), echo = ec) except Exception as ex: pass import traceback print('--- Caught Exception ---') traceback.print_exc() print('------------------------') import sys import platform print(sys.version) try: dx = DXLProtocolV2('\\.\COM12', 3000000) except: pass else: th1 = Thread(target = func1, args = (dx,)) th2 = Thread(target = func2, args = (dx,)) th1.start() th2.start() th1.join() th2.join() del th1, th2, dx try: dx = DXLProtocolV1('\\.\COM12', 57600) except: dx = None else: th3 = Thread(target = func3, args = (dx,)) th3.start() th3.join() del th3, dx print('fin')
Python2ついでにMicroPythonの互換性レベルを図ってみた。
欲しいライブラリが用意されていない以上に、基本的な部分が結構緩い。また実際に動かすとそれなりに遅い。それでもMicroPythonは存在しているし、個人的にはかつてのBASICの感覚が蘇ってきたので、ちょっとした使い方であれば選択肢に入れても良いかなと。
ひとまずPicoSHIELD+Raspberry Pi Pico上で動くように修正してみた。オーバーロックしているのはご愛敬として、マルチスレッドを除外した程度で実質PC版と大差無い。@micropython.viperを多用した時はかなり高速化できていたのだが、ここはコードの互換性を優先。
# pyDXL.py for MicroPython(RP2040) # (C) 2024 T.Uemitsu from machine import UART, Pin import time, array from collections import namedtuple from struct import pack, unpack ########################################################## # Functionalized the part of converting int to bytes. # If specified as a tuple, it is converted to bytes at once. ########################################################## @micropython.native def B2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return bytes(((d & 0x7f) | 0x80) if d < 0 else d for d in d) else: return bytes(pack('<B', ((d & 0x7f) | 0x80) if d < 0 else d)) @micropython.native def W2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<H',d) for d in [((d & 0x7fff) | 0x8000) if d < 0 else d for d in d]]) else: return bytes(pack('<H', ((d & 0x7fff) | 0x8000) if d < 0 else d)) @micropython.native def L2Bs(d) -> bytes: if isinstance(d, list) or isinstance(d, tuple): return b''.join([pack('<I',d) for d in [((d & 0x7fffffff) | 0x80000000) if d < 0 else d for d in d]]) else: return bytes(pack('<I', ((d & 0x7fffffff) | 0x80000000) if d < 0 else d)) ########################################################## # API for Dynamixel protocol V1 ########################################################## class DXLProtocolV1: BROADCASTING_ID = 0xfe INST_PING = 0x01 INST_READ = 0x02 INST_WRITE = 0x03 INST_REG_WRITE = 0x04 INST_ACTION = 0x05 INST_FACTORY_RESET = 0x06 INST_REBOOT = 0x08 INST_SYNC_WRITE = 0x83 INST_SYNG_REG_WRITE = 0x85 TSyncW = namedtuple("TSyncW", ("id", ("data"))) def __init__(self, baudrate = 57600, timeout_ms = 50): """ Initalize parameters ------------- port : int UART no. baudrate : int Serial baudrate[bps] timeout : float Read timeout[s] """ self.__Error = 0 self.__baudrate = baudrate self.__timeout = timeout_ms self.__serial = UART(1, baudrate, tx = Pin(4), rx = Pin(5), txbuf = 500, rxbuf = 500, timeout = timeout_ms) @property def baudrate(self): return self.__baudrate @baudrate.setter def baudrate(self, baudrate): self.__baudrate = baudrate self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def timeout(self): return self.__timeout @timeout.setter def timeout(self, timeout_ms): self.__timeout = timeout_ms self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def Error(self): return self.__Error @micropython.native def TxPacket(self, id : int, inst : int, param : bytes, echo = False) -> (bytes, bool): """ Sending packets parameters ------------- id : int Target ID inst : int Instruction command param : bytes Packet parameters Returns ------- bytes Packets sent bool Success or failure """ if ((id == self.BROADCASTING_ID) or (id >= 0 and id <= 253)) and len(param) <= (256 - 6): instp = bytearray([0xff,0xff,id,0,inst]) + bytes(param) instp[3] = len(instp) - 3 instp += B2Bs(~sum(instp[2:]) & 0xff) _ = self.__serial.read(self.__serial.any()) if echo: print('TX:', instp.hex(':')) self.__serial.write(instp) return bytes(instp), True return None, False @micropython.native def __rx(self, length) -> bytes: s = self.__serial.read(length) if s: l = len(s) if l == length: return s else: r = s length -= l while self.__serial.any() > 0: s = self.__serial.read(length) r += s length -= len(s) if length == 0: break return r else: return bytes() @micropython.native def RxPacket(self, echo = False) -> (bytes, bool): """ Receiving packets Returns ------- bytes Packets received bool Success or failure """ self.__serial.flush() statp = self.__rx(5) if statp: if len(statp) == 5: l = statp[3] - 1 self.__Error = statp[4] statp += self.__rx(l) if len(statp) == l + 5: if statp[-1:][0] == ((~sum(statp[2:-1])) & 0xff): if echo: print('RX:', statp.hex(':')) return bytes(statp), (statp[4] & 0x40) == 0 return None, False @micropython.native def Write(self, id : int, addr : int, data : bytes, echo = False) -> bool: """ Write instruction parameters ------------- id : int Target ID addr : int Target item address data : bytes Data to be written Returns ------- result : bool Success or failure """ if id >= 0 and id <= self.BROADCASTING_ID and addr >= 0 and addr <= 254: if self.TxPacket(id, self.INST_WRITE, B2Bs(addr) + data, echo)[1]: if id != self.BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return dat[2] == id and (dat[4] & 0x18) == 0 else: return True return False @micropython.native def Write8(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, B2Bs(data), echo) @micropython.native def Write16(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, W2Bs(data), echo) @micropython.native def Write32(self, id : int, addr : int, data : int | tuple | list, echo = False) -> bool: return self.Write(id, addr, L2Bs(data), echo) @micropython.native def Read(self, id : int, addr : int, length : int, echo = False) -> (bytes, bool): """ Read instruction parameters ------------- id : int Target ID addr : int Target item address length : int Number of bytes to read Returns ------- bytes Data read bool Success or failure """ if id >= 0 and id <= 253 and addr >= 0 and addr <= 254 and length > 0 and length <= (256 - 6): if self.TxPacket(id, self.INST_READ, B2Bs((addr, length)), echo)[1]: dat, r = self.RxPacket(echo) if r: if dat[2] == id and (dat[4] & 0x8) == 0: return bytes(dat[5:-1]) return None @micropython.native def Read8(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, length, echo) if r != None: n = unpack('<' + (('b'*length) if signed else ('B'*length)), r[0:1 * length]) return n if length > 1 else n[0] return None @micropython.native def Read16(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 2 * length, echo) if r != None: n = unpack('<' + (('h'*length) if signed else ('H'*length)), r[0:2 * length]) return n if length > 1 else n[0] return None @micropython.native def Read32(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 4 * length, echo) if r != None: n = unpack('<' + (('i'*length) if signed else ('I'*length)), r[0:4 * length]) return n if length > 1 else n[0] return None @micropython.native def SyncWrite(self, addr : int, length : int, id_datas : (TSyncW), echo = False) -> bool: """ Sync Write instruction parameters ------------- addr : int Target item address length : int Number of bytes to write id_datas : (TSyncW) Target ID and data Returns ------- bool Success or failure """ if addr >= 0 and addr <= 254 and length > 0 and length < (256 - 6): param = B2Bs((addr,length)) for d in id_datas: param += B2Bs(d.id) + d.data if len(d.data) != length or d.id < 0 or d.id > 253: del param return False del param return self.TxPacket(self.BROADCASTING_ID, self.INST_SYNC_WRITE, param, echo)[1] return False @micropython.native def Ping(self, id : int, echo = False) -> bool: if self.TxPacket(id, self.INST_PING, bytes(), echo): dat, r = self.RxPacket(echo) if r: return id == dat[2] and dat[3] == 2 return False @micropython.native def FactoryReset(self, id : int, echo = False) -> bool: if self.TxPacket(id, self.INST_RESET, bytes(), echo): dat, r = self.RxPacket(echo) if r: return id == dat[2] and dat[3] == 2 return False @micropython.native def Reboot(self, id : int, echo = False) -> bool: if self.TxPacket(id, self.INST_REBOOT, bytes(), echo): dat, r = self.RxPacket(echo) if r: return id == dat[2] and dat[3] == 2 return False ########################################################## # API for Dynamixel protocol V2 ########################################################## class DXLProtocolV2: BROADCASTING_ID = const(0xfe) INST_PING = const(0x01) INST_READ = const(0x02) INST_WRITE = const(0x03) INST_REG_WRITE = const(0x04) INST_ACTION = const(0x05) INST_FACTORY_RESET = const(0x06) INST_REBOOT = const(0x08) INST_SYS_WRITE = const(0x0d) INST_CLEAR = const(0x10) INST_CONTROL_TABLE_BACKUP = const(0x20) INST_STATUS = const(0x55) INST_SYNC_READ = const(0x82) INST_SYNC_WRITE = const(0x83) INST_SYNG_REG_WRITE = const(0x85) INST_FAST_SYNC_READ = const(0x8a) INST_BULK_READ = const(0x92) INST_BULK_WRITE = const(0x93) INST_FAST_BULK_READ = const(0x9a) TSyncW = namedtuple("TSyncW", ("id", ("data"))) TBulkW = namedtuple("TBulkW", ("id", "addr", ("data"))) TBulkR = namedtuple("TBulkR", ("id", "addr", "length")) __crc16_lutable = array.array('H', ( 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 )) def __init__(self, baudrate = 57600, timeout_ms = 50): """ Initalize parameters ------------- port : int UART no. baudrate : int Serial baudrate[bps] timeout : float Read timeout[s] """ self.__Error = 0 self.__baudrate = baudrate self.__timeout = timeout_ms self.__serial = UART(1, baudrate, tx = Pin(4), rx = Pin(5), txbuf = 500, rxbuf = 500, timeout = timeout_ms) @property def baudrate(self): return self.__baudrate @baudrate.setter def baudrate(self, baudrate): self.__baudrate = baudrate self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def timeout(self): return self.__timeout @timeout.setter def timeout(self, timeout_ms): self.__timeout = timeout_ms self.__serial = UART(1, baudrate = self.__baudrate, tx = Pin(4), rx = Pin(5), rxbuf = 500, timeout = self.__timeout) @property def Error(self): return self.__Error @micropython.viper def __crc16(self, dat : ptr8, l:int) -> int: crc:int = 0 pt = ptr16(self.__crc16_lutable) for i in range(l): crc = (crc << 8) ^ pt[(((crc >> 8) ^ dat[i]) & 0xff)] return crc & 0xffff @micropython.native def TxPacket(self, id : int, inst : int, param : bytes, echo = False) -> (bytes, bool): """ Sending packets parameters ------------- id : int Target ID inst : int Instruction command param : bytes Packet parameters Returns ------- bytes Packets sent bool Success or failure """ if ((id == self.BROADCASTING_ID) or (id >= 0 and id <= 252)) and len(param) < (65536 - 10): instp = bytearray([0xff,0xff,0xfd,0x00,id,0,0,inst]) + bytearray(param).replace(b'xffxffxfd',b'xffxffxfdxfd') instp[5:7] = W2Bs(len(instp) - 5) instp += W2Bs(self.__crc16(instp, len(instp))) _ = self.__serial.read(self.__serial.any()) if echo: print('TX:', instp.hex(':')) self.__serial.write(instp) return bytes(instp), True return None, False @micropython.native def __rx(self, length) -> bytes: s = self.__serial.read(length) if s: l = len(s) if l == length: return s else: r = s length -= l while self.__serial.any() > 0: s = self.__serial.read(length) r += s length -= len(s) if length == 0: break return r else: return bytes() @micropython.native def RxPacket(self, echo = False) -> (bytes, bool): """ Receiving packets Returns ------- bytes Packets received bool Success or failure """ self.__serial.flush() statp = self.__rx(9) if statp: if len(statp) == 9: if statp[0] == 0xff and statp[1] == 0xff and statp[2] == 0xfd and statp[3] == 0 and statp[7] == 0x55: l = unpack('<H', statp[5:7])[0] - 2 self.__Error = statp[8] statp += self.__rx(l) if len(statp) == l + 9: if unpack('<H', statp[-2:])[0] == self.__crc16(statp[:-2], len(statp[:-2])): statp = statp[0:9] + statp[9:].replace(b'xffxffxfdxfd',b'xffxffxfd') if echo: print('RX:', statp.hex(':')) return bytes(statp), (statp[8] & 0x7f) == 0 return None, False @micropython.native def Write(self, id : int, addr : int, data: bytes, echo = False) -> bool: """ Write instruction parameters ------------- id : int Target ID addr : int Target item address data : bytes Data to be written Returns ------- result : bool Success or failure """ if ((id >= 0 and id <= 252) or (id == self.BROADCASTING_ID)) and addr >= 0 and addr <= 65535: if self.TxPacket(id, self.INST_WRITE, W2Bs(addr) + data, echo)[1]: if id != self.BROADCASTING_ID: dat, r = self.RxPacket(echo) if r: return dat[4] == id and (dat[8] & 0x7f) == 0 else: return True return False @micropython.native def Write8(self, id : int, addr : int, data : int | tuple | list, echo = False): return self.Write(id, addr, B2Bs(data), echo) @micropython.native def Write16(self, id : int, addr : int, data : int | tuple | list, echo = False): return self.Write(id, addr, W2Bs(data), echo) @micropython.native def Write32(self, id : int, addr : int, data : int | tuple | list, echo = False): return self.Write(id, addr, L2Bs(data), echo) @micropython.native def Read(self, id : int, addr : int, length : int, echo = False) -> (bytes, bool): """ Read instruction parameters ------------- id : int Target ID addr : int Target item address length : int Number of bytes to read Returns ------- bytes Data read bool Success or failure """ if id >= 0 and id <= 252 and addr >= 0 and addr <= 65535 and length > 0 and length < (65536 - 10): if self.TxPacket(id, self.INST_READ, W2Bs(addr) + W2Bs(length), echo)[1]: dat, r = self.RxPacket(echo) if r: return bytes(dat[9:-2]) return None @micropython.native def Read8(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, length, echo) if r: n = unpack('<' + (('b'*length) if signed else ('B'*length)), r[0:1 * length]) return n if length > 1 else n[0] return None @micropython.native def Read16(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 2 * length, echo) if r != None: n = unpack('<' + (('h'*length) if signed else ('H'*length)), r[0:2 * length]) return n if length > 1 else n[0] return None @micropython.native def Read32(self, id : int, addr : int, length = 1, signed = False, echo = False) -> int: r = self.Read(id, addr, 4 * length, echo) if r != None: n = unpack('<' + (('i'*length) if signed else ('I'*length)), r[0:4 * length]) return n if length > 1 else n[0] return None @micropython.native def SyncWrite(self, addr : int, length : int, id_datas : (TSyncW), echo = False) -> bool: """ Sync Write instruction parameters ------------- addr : int Target item address length : int Number of bytes to write id_datas : (TSyncW) Target ID and data Returns ------- bool Success or failure """ if addr >= 0 and addr <= 65535 and length > 0 and length < (65536 - 10): param = W2Bs(addr) + W2Bs(length) for d in id_datas: param += bytes((d.id,)) + d.data if len(d.data) != length or d.id < 0 or d.id > 252: del param return False return self.TxPacket(self.BROADCASTING_ID, self.INST_SYNC_WRITE, param, echo)[1] return False @micropython.native def SyncRead(self, addr : int, length : int, ids : (int), echo = False) -> tuple: """ Sync Read instruction parameters ------------- addr : int Target item address length : int Number of bytes to read ids : (int) Target IDs Returns ------- tuple Read ID and data """ result = () if addr >= 0 and addr < 65535 and length > 0 and length < (65536 - 10): if self.TxPacket(self.BROADCASTING_ID, self.INST_SYNC_READ, W2Bs(addr) + W2Bs(length) + B2Bs(ids), echo)[1]: for id in ids: dat, r = self.RxPacket(echo) if r: if dat[4] == id: result += (id, bytes(dat[9:9 + length])), else: result += (id, bytes([])), else: result += (id, bytes([])), return result @micropython.native def BulkWrite(self, data : (TBulkW), echo = False) -> bool: """ Bulk Write instruction parameters ------------- addr : int Target item address length : int Number of bytes to write ids : (TSyncW) Target ID and data Returns ------- bool Success or failure """ param = bytes() for d in data: if d.id >= 0 and d.id <= 252 and d.addr >= 0 and d.addr <= 65535: param += B2Bs(d.id) + W2Bs(d.addr) + W2Bs(len(d.data)) + d.data else: del param return False return self.TxPacket(self.BROADCASTING_ID, self.INST_BULK_WRITE, param, echo)[1] @micropython.native def BulkRead(self, data:(TBulkR), echo = False) -> tuple: """ Bulk Read instruction parameters ------------- data : (TBulkR) ID, address, and number of bytes to be read Returns ------- tuple Read ID and data """ result = () param = bytes() for d in data: if d.id < 0 or d.addr < 0 or d.length < 0: return result param += B2Bs(d.id) + W2Bs(d.addr) + W2Bs(d.length) if self.TxPacket(self.BROADCASTING_ID, self.INST_BULK_READ, param, echo)[1]: for d in data: rxd, r = self.RxPacket(echo) if r: if(d.id == rxd[4]): result += (d.id, bytes(rxd[9:-2])), else: result += (d.id, bytes([])), else: result += (d.id, bytes([])), del param return result @micropython.native def Ping(self, id : int, echo = False) -> bool: if self.TxPacket(id, self.INST_PING, bytes(), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 7 and rxd[6] == 0 return False @micropython.native def FactoryReset(self, id : int, p1 : int, echo = False) -> bool: if self.TxPacket(id, self.INST_FACTORY_RESET, B2Bs(p1), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 4 and rxd[6] == 0 return False @micropython.native def Reboot(self, id : int, echo = False) -> bool: if self.TxPacket(id, self.INST_REBOOT, bytes(), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 4 and rxd[6] == 0 return False @micropython.native def Clear(self, id : int, val, echo = False) -> bool: if self.TxPacket(id, self.INST_CLEAR, B2Bs(1) + L2Bs(val), echo)[1]: rxd, r = self.RxPacket(echo) if r: return id == rxd[4] and rxd[5] == 4 and rxd[6] == 0 return False ########################################################## # test code ########################################################## if __name__ == "__main__": import uasyncio as asyncio from ucontextlib import contextmanager import gc try: led = Pin('LED', Pin.OUT) except TypeError: led = Pin(25, Pin.OUT) led.off() dx = None ec = False ID = 1 fin = False async def test1(): global dx, ec, ID, fin @contextmanager def stopwatch(): led.toggle() start_time = time.ticks_us() yield #print('..proc time={:.2f}ms {}'.format((time.ticks_us() - start_time) / 1000, gc.mem_alloc())) # Basic operation check try: ''' with stopwatch(): r = dx.FactoryReset(ID, echo=ec) print(f'Reset({ID}): {r}') dx.baudrate = 57600 ID = 1 await asyncio.sleep(0.5) ''' """ ping dxl """ for i in range(10): with stopwatch(): print(f'Ping({i}):', dx.Ping(i, echo=ec)) """ reboot dxl """ with stopwatch(): r = dx.Reboot(ID, echo=ec) print(f'Reboot({ID})= {r}') await asyncio.sleep(0.5) """ basic packet proc """ with stopwatch(): r0 = dx.TxPacket(ID, dx.INST_WRITE, (65, 0, 1)) r1 = dx.RxPacket() print('TxPacket({})= {} {}'.format(ID, r0[1], r0[0].hex(':') if r0[0] else '--')) print('RxPacket({})= {} {}'.format(ID, r1[1], r1[0].hex(':') if r1[0] else '--')) """ dump memory """ l = 50 for addr in range(0, 700, l): r = dx.Read(ID, addr, l, echo = ec) print(f'Read({addr};{l})=',r.hex(':') if r else '!!!!!!!!!!!!!!!!!!!!!!!!') """ read byte item """ with stopwatch(): r = dx.Read8(ID, 65, echo=ec) print(f'Read8({ID},65)= {r}') """ read word item """ with stopwatch(): r = dx.Read16(ID, 120, echo=ec) print(f'Read16({ID},120)= {r}') """ read long item """ with stopwatch(): r = dx.Read32(ID, 132, echo=ec) print(f'Read32({ID},132)= {r}') """ sync read inst. """ with stopwatch(): d = dx.SyncRead(0, 4, (1, 2, 3, 4, 5, 6, 7, 8), echo=ec) print('SyncRead=') for d in d: print(f' ({d[0]},0)', d[1].hex(':') if len(d[1]) > 0 else ()) """ sync write inst. """ for i in range(20): with stopwatch(): dx.SyncWrite(65, 1, (dx.TSyncW(1, B2Bs(1)), dx.TSyncW(2, B2Bs(1)), dx.TSyncW(3, B2Bs(1))), echo=ec) await asyncio.sleep(0.05) with stopwatch(): dx.SyncWrite(65, 1, (dx.TSyncW(1, B2Bs(0)), dx.TSyncW(2, B2Bs(0)), dx.TSyncW(3, B2Bs(0))), echo=ec) await asyncio.sleep(0.05) """ set goal position """ #torque off with stopwatch(): dx.Write8(ID, 64, 0, echo=ec) #multi turn with stopwatch(): dx.Write8(ID, 11, 4, echo=ec) #torque on with stopwatch(): dx.Write8(ID, 64, 1, echo=ec) for gp in tuple(range(2047, 2047 + 4096, 64)) + tuple(range(2047 + 4096, 2047 - 4096, -64)) + tuple(range(2047 - 4096, 2047, 64)): #set goal potition with stopwatch(): dx.Write32(ID, 116, gp, echo=ec) #get present potition with stopwatch(): pp = dx.Read32(ID, 132, signed = True, echo=ec) if pp: print(f'Goal Pos={gp:6d}, Present Pos={pp:6d}', end='r') else: break await asyncio.sleep(0.05) else: print('') #torque off with stopwatch(): dx.Write8(ID, 64, 0, echo=ec) #normal turn with stopwatch(): dx.Write8(ID, 11, 3, echo=ec) """ test add/remove suffixes """ fffffd = bytes(( 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, 0xff,0xff,0xfd, )) with stopwatch(): r = dx.Write(ID, 634, fffffd, echo=ec) print(f'Write({ID},634)= {r}') with stopwatch(): r = dx.Read(ID, 634, len(fffffd), echo=ec) print(f'Read({ID},634)=', r.hex(':') if r else '--') """ bulk read inst. """ with stopwatch(): d = dx.BulkRead((dx.TBulkR(1, 0, 10), dx.TBulkR(2, 0, 20), dx.TBulkR(3, 0, 30), dx.TBulkR(4, 0, 40), dx.TBulkR(6, 0, 50)), echo=ec) print('BulkRead=') for d in d: print(f' ({d[0]},0)', d[1].hex(':')) """ bulk write inst. """ dx.Write8(ID, 64, 1, echo=ec) with stopwatch(): dx.BulkWrite((dx.TBulkW(1, 104, L2Bs((0,0,0,1024))), dx.TBulkW(2, 65, B2Bs(0)), dx.TBulkW(3, 65, B2Bs(0))), echo=ec) await asyncio.sleep(0.5) with stopwatch(): dx.BulkWrite((dx.TBulkW(1, 104, L2Bs(0)+L2Bs(0)+L2Bs(0)+L2Bs(2048)), dx.TBulkW(2, 65, B2Bs(1)), dx.TBulkW(3, 65, B2Bs(1))), echo=ec) await asyncio.sleep(0.5) with stopwatch(): dx.BulkWrite((dx.TBulkW(1, 104, L2Bs(0)+L2Bs(0)+L2Bs(0)+L2Bs(1024)), dx.TBulkW(2, 65, B2Bs(0)), dx.TBulkW(3, 65, B2Bs(0))), echo=ec) await asyncio.sleep(0.5) with stopwatch(): dx.BulkWrite((dx.TBulkW(1, 116, L2Bs(2048)), dx.TBulkW(2,65, B2Bs(1)), dx.TBulkW(3, 65, B2Bs(1))), echo=ec) await asyncio.sleep(0.5) with stopwatch(): dx.BulkWrite((dx.TBulkW(1, 65, B2Bs(0)), dx.TBulkW(2, 65, B2Bs(0)), dx.TBulkW(3, 65, B2Bs(0))), echo=ec) dx.Write8(ID, 64, 0, echo=ec) except Exception as ex: print('--- Caught Exception ---') import sys sys.print_exception(ex) print('------------------------') fin = True async def test2(): global dx, ec, ID, fin await asyncio.sleep(0.5) while not fin: try: dx.Write8(ID, 65, dx.Read8(ID, 65, echo=ec) ^ 1, echo=ec) await asyncio.sleep(0.02) except Exception as ex: print('--- Caught Exception ---') import sys sys.print_exception(ex) print('------------------------') def test3(): global dx, ec, ID, fin # Basic operation check print('reset=', dx.Reboot(1, echo=ec)) time.sleep(2) print('ping=', dx.Ping(1, echo=ec)) dx.TxPacket(1, dx.INST_READ, (0x2b, 0x01), True) dx.RxPacket(True) r = dx.Read(1, 0, 70, echo=ec) print(r.hex(',')) print('len=',len(r)) for i in range(15): led = dx.Read8(1, 25, echo=ec) ^ 1 dx.Write8(1, 25, led, echo=ec) print(f'led={led}', end = 'r') time.sleep(0.2) print() dx.Write8(1, 24, 0, echo=ec) dx.Write16(1, 6, (4095, 4095), echo=ec) for gp in tuple(range(2047, 2047 + 4096, 64)) + tuple(range(2047 + 4096, 2047 - 4096, -64)) + tuple(range(2047 - 4096, 2047, 64)): dx.Write16(1, 30, gp, echo=ec) pp = dx.Read16(1, 36, signed=True, echo=ec) print(f'Goal Pos={gp:6d}, Present Pos={pp:6d}', end='r') time.sleep(0.05) else: print('') dx.Write8(1, 24, 0, echo=ec) dx.Write16(1, 6, (0, 4095),echo=ec) machine.freq(250000000) # set the CPU frequency to 250 MHz dx = DXLProtocolV2(baudrate = 3000000) loop = asyncio.get_event_loop() loop.create_task(test1()) loop.create_task(test2()) loop.run_forever() time.sleep(1) dx = DXLProtocolV1(baudrate = 57600) test3()
$ bitbake -c savedefconfig linux-renesas
¶以下にこちらでbitmakeした生成ファイル(ディスクイメージ・u-boot)を晒しておく。7zipで圧縮してあるので展開した上で使用する事。ブラウザのダウンロードでは途中で中断したファイルが生成される可能性が高いので、wget等のダウンローダを使う事をお勧めする。
¶ターミナル上に赤い文字と共にERRORが表示されていなければ、「~/yocto_rzboard/build/tmp/deploy/images/rzboard」の中に成果物が生成されている。avnet-core-image-rzboard-~.wicができていればまずはOK。もしrenesas社から提供されるアーカイブファイルが更新されているのを見かけたら、更新された部分のみcreate_yocto_rz_src.shを修正しスクリプトを実行すれば良い。エラーが出て止まっていた場合はbitbakeを繰り返すことで成功する事が多いが、どうにもならない場合は原因を特定するスキルが無い限り真っさらからやり直した方が早道かも。
Windowsを使ってwicファイルをSDカードに書き込む場合は、Win32 Disk Imagerを使えばwicファイルと書き込み先のSDカードのドライブを指定するだけなので簡単。ubuntuを使ってSDカードへ書き込む場合は、bmaptoolを準備をしておくと良いかも(と書いておきながらbmaptoolで書き込んだSDカードは時々起動に失敗するのでダメかも)。
以下にこちらでbitmakeしたイメージを晒しておく。2つに分けた深い理由は無いのと、7zipで圧縮してあるので展開した上で使用する事。ブラウザのダウンロードでは途中で中断したファイルが生成される可能性が高いので、wget等のダウンローダを使う事をお勧めする。
RZboard V2Lに触れる機会があり、その際に聞かれて操作を促した内容を紹介。あくまで必要に迫られた時の操作に限定しているため、yoctoが具体的にどうのこうのやらDRP-AIの環境整備といったネタには触れていない。
にわかでやるための、ありきたりかつ間違えようの無い操作のみを雑多に記載しているため、後々はこちらのリソースが参考になるかと。
いずれにせよRZ/V2LはDRP-AIが主たる目的で、Linuxはその介添えに過ぎない。OSが動いても余力がある訳ではないので、遊び甲斐は無い。
Yocto Projectに目を通すまでも無く、以下の手順を踏めば基本的にブートイメージが生成できる事になっている。
https://github.com/Avnet/meta-rzboard/tree/rzboard_dunfell_5.10_v3
ここではビルド環境としてUbuntu 20.04 LTSを新規でインストールしたPCを使ってみた。なお使い込んだOSだとうまくいかないようで、真っ新な状態でUbuntuをインストールし直せば大抵は解消するようだ。ちなみに最新のUbuntuは以ての外である。
大半の操作はCUI上で行う。まずはUbuntu上へ必要なツール類をインストール。ちなみに「$」はターミナルのプロンプトのつもりなので、入力するものではない。もちろんインターネットに接続している前提。
$ sudo apt install -y gawk wget git-core diffstat unzip texinfo gcc-multilib build-essential chrpath socat cpio python3 python3-pip python3-pexpect xz-utils debianutils iputils-ping libsdl1.2-dev xterm p7zip-full libyaml-dev rsync curl locales bash-completion
次にrenesasから提供されるドライバやライブラリ等の圧縮ファイルを展開するためのスクリプトをダウンロード。
$ cd ~/ $ wget https://raw.githubusercontent.com/Avnet/meta-rzboard/rzboard_dunfell_5.10_v3/tools/create_yocto_rz_src.sh
ダウンロードしたcreate_yocto_rz_src.shに記述されているパッケージだが、renesasのサイトで公開されているものの方が新しい場合がある。2023/10/11現在以下が最新の模様(簡便化のため直リンクご容赦、リンク先はrenesasのアカウントが必要)。適宜create_yocto_rz_src.shを修正。
スクリプトを実行し、更に必要なファイルをgit cloneしたらおもむろにイメージを生成(bitbake)。bitbakeはPCのリソースをフルに使う処理なので、PCは良く冷やす事。今回使ったPCでは夜寝る前に仕掛けて朝起きたら終わっていた感じだが、ここ最近のマルチコアのPCを使っても数時間かかるかと。
$ cd ~ $ bash create_yocto_rz_src.sh $ cd ~/yocto_rzboard/ $ git clone https://github.com/Avnet/meta-rzboard.git -b rzboard_dunfell_5.10_v3 $ mkdir -p ./build/conf $ cp meta-rzboard/conf/rzboard/* build/conf/ $ source poky/oe-init-build-env build/ $ bitbake avnet-core-image
ターミナル上に赤い文字と共にERRORが表示されていなければ、「~/yocto_rzboard/build/tmp/deploy/images/rzboard」の中に成果物が生成されている。avnet-core-image-rzboard-~.wicができていればまずはOK。もしrenesas社から提供されるアーカイブファイルが更新されているのを見かけたら、更新された部分のみcreate_yocto_rz_src.shを修正しスクリプトを実行すれば良い。エラーが出て止まっていた場合はbitbakeを繰り返すことで成功する事が多いが、どうにもならない場合は原因を特定するスキルが無い限り真っさらからやり直した方が早道かも。
これぐらいの数とサイズのファイルが含まれるので、HDDの空き容量には注意。
最後にwicファイルをRZBboard上のeMMCに書き込むのだが、USBシリアルコンバータの準備が面倒だったので、ここではSDカードにイメージを書き込んで済ませた。
Windowsを使ってwicファイルをSDカードに書き込む場合は、Win32 Disk Imagerを使えばwicファイルと書き込み先のSDカードのドライブを指定するだけなので簡単。ubuntuを使ってSDカードへ書き込む場合は、bmaptoolを準備をしておくと良いかも(と書いておきながらbmaptoolで書き込んだSDカードは時々起動に失敗するのでダメかも)。
RZBoardをSDカードでブートさせる際はSW1-1とSW1-2は共にOFFで。
以下にこちらでbitmakeした生成ファイル(ディスクイメージ・u-boot)を晒しておく。7zipで圧縮してあるので展開した上で使用する事。ブラウザのダウンロードでは途中で中断したファイルが生成される可能性が高いので、wget等のダウンローダを使う事をお勧めする。
概ね「~/yocto_rzboard/build/conf/local.conf」のへの追記で対応。Python好いている御仁向けに少しモジュールを追加、Dropbear SSHでは何かと不都合があったのでOpenSSHに入れ替え、timezoneが無いので追加、guiアプリを作ってみたかったのでgtk+3追加、またbmaptool使ってイメージを書き込む際に使われるブロックマップの生成も促す。
IMAGE_INSTALL_append = " python3-pip python3-pyserial python3-numpy python3-psutil python3-pygobject gtk+3" IMAGE_INSTALL_append = " packagegroup-core-ssh-openssh openssh-sftp-server" IMAGE_INSTALL_append = " tzdata" DEFAULT_TIMEZONE = "Asia/Tokyo" IMAGE_FSTYPES += " wic.bmap"
追記したらイメージ生成。
$ cd ~/yocto_rzboard $ source poky/oe-init-build-env build/ $ bitbake avnet-core-image
その他のパッケージ追加も大抵はこの手でOKな事が多い。
日本語のサポートは含まれていないので、日本語を含むファイルの扱いが困難。文字化けする程度であれば無視すれば済むが、日本語を含むファイルの編集となると少々難儀である。GUIを含めて日本語を表示できるようにするにはlocal.confに次を追記してbitbakeする。
IMAGE_LINGUAS ?= "ja-jp ja-jp.euc-jp" GLIBC_GENERATE_LOCALES = "en_GB.UTF-8 en_US.UTF-8 ja_JP.UTF-8 ja_JP.EUC-JP" IMAGE_INSTALL_append = " ttf-vlgothic ttf-sazanami-gothic ttf-sazanami-mincho"
起動したらlocalectlでロケールを設定したら一度再起動。リモートターミナルの文字コードがUTF-8になってさえいれば、Linuxが再起動した後から一部コマンドのヘルプが日本語化され、UTF-8で記述されたファイルの編集や閲覧が文字化けしなくなる。
$ export LC_ALL=ja_JP.UTF-8 $ localectl set-locale LANG=ja_JP.UTF-8 $ reboot
weston-terminalも日本語表示に対応する。
USB機器のデバイスドライバ絡みであれば、menuconfigで済む場合が多い。
$ cd ~/yocto_rzboard $ source poky/oe-init-build-env build/ $ bitbake -c menuconfig linux-renesas
編集が終わったら「.config」として保存。
再びイメージ生成。大して時間はかからない。
$ bitbake -c savedefconfig linux-renesas $ bitbake avnet-core-image
menuconfigするのに何か足りなければ追加しておく。
$ sudo apt install tmux libevent-dev ncurse-*
デフォルトでは必要最低限程度のサイズでイメージが生成されるため、残りのSDカードの容量は未使用状態である。Hardware User Guideの12.3に記載されている操作を行えば、SDカードの最大サイズまで拡張できる。なお態々debug connectorを経由して操作する必要は無い。
root@rzboard:~# df -h ファイルシス サイズ 使用 残り 使用% マウント位置 /dev/root 3.5G 2.8G 468M 86% / devtmpfs 426M 4.0K 426M 1% /dev tmpfs 651M 0 651M 0% /dev/shm tmpfs 651M 9.9M 641M 2% /run tmpfs 651M 0 651M 0% /sys/fs/cgroup tmpfs 651M 0 651M 0% /tmp tmpfs 651M 24K 651M 1% /var/volatile /dev/mmcblk0p1 100M 22M 78M 22% /boot tmpfs 131M 0 131M 0% /run/user/0 root@rzboard:~# fdisk /dev/mmcblk0 fdisk (util-linux 2.35.1) へようこそ。 ここで設定した内容は、書き込みコマンドを実行するまでメモリのみに保持されます。 書き込みコマンドを使用する際は、注意して実行してください。 コマンド (m でヘルプ): p ディスク /dev/mmcblk0: 29.74 GiB, 31914983424 バイト, 62333952 セクタ 単位: セクタ (1 * 512 = 512 バイト) セクタサイズ (論理 / 物理): 512 バイト / 512 バイト I/O サイズ (最小 / 推奨): 512 バイト / 512 バイト ディスクラベルのタイプ: dos ディスク識別子: 0xcb176a81 デバイス 起動 開始位置 終了位置 セクタ サイズ Id タイプ /dev/mmcblk0p1 * 32 204831 204800 100M c W95 FAT32 (LBA) /dev/mmcblk0p2 204832 7824703 7619872 3.6G 83 Linux コマンド (m でヘルプ): d パーティション番号 (1,2, 既定値 2): 2 パーティション 2 を削除しました。 コマンド (m でヘルプ): n パーティションタイプ p 基本パーティション (1 primary, 0 extended, 3 free) e 拡張領域 (論理パーティションが入ります) 選択 (既定値 p): p パーティション番号 (2-4, 既定値 2): 2 最初のセクタ (204832-62333951, 既定値 206848): 204832 最終セクタ, +/-セクタ番号 または +/-サイズ{K,M,G,T,P} (204832-62333951, 既定値 62333951): 新しいパーティション 2 をタイプ Linux、サイズ 29.6 GiB で作成しました。 パーティション #2 には ext4 署名が書き込まれています。 署名を削除しますか? [Y]es/[N]o: N コマンド (m でヘルプ): w パーティション情報が変更されました。 ディスクを同期しています。 root@rzboard:~# resize2fs /dev/mmcblk0p2 resize2fs 1.45.7 (28-Jan-2021) Filesystem at /d[ 662.106753] EXT4-fs (mmcblk0p2): resizing filesystem from 952484 to 7766140 blocks ev/mmcblk0p2 is [ 662.115758] EXT4-fs (mmcblk0p2): resizing filesystem from 952484 to 7766016 blocks mounted on /; on-line resizing required old_desc_blocks = 1, new_desc_blocks = 4 [ 664.033021] EXT4-fs (mmcblk0p2): resized filesystem to 7766016 The filesystem on /dev/mmcblk0p2 is now 7766140 (4k) blocks long. root@rzboard:~#
UARTのデバッグポート(J19)を使わずに全ての操作を完結させることができないので、手持ちのUSBシリアル変換モジュールを使用。通信にかかる3つの端子は配列が一致しているので、適当なメスピンヘッダをモジュールにハンダしてJ19に装着。
デフォルトでは「/boot/uEnv.txt」の記載内容が少ないので追記。特にmacアドレス(ethaddr)を設定しないままだと起動の度にランダムな値になり、それに伴いルーターからのIPアドレスの割り当てがコロコロ変わって少し面倒。
fdtfile=rzboard.dtb enable_overlay_cm33=1 enable_overlay_disp=hdmi enable_overlay_uart2=yes enable_overlay_i2c=yes enable_overlay_gpio=yes enable_overlay_adc=yes enable_overlay_audio=yes #fdt_extra_overlays=rzboard-mipi-ph720128t003.dtbo #fdt_extra_overlays=1.dtbo 2.dtbo 3.dtbo ethaddr=AA:BB:CC:DD:EE:FF
westonにはターミナルしか登録されていないので寂しいが、gui絡みのアプリケーションを実行した際の出力先になる。「/etc/xdg/weston/weston.ini」に以下を追記しておくと良い場合は設定すべし。
[output] name=HDMI-A-1 mode=1280x720@60 #mode=1920x1080@60 [keyboard] keymap_rules=evdev keymap_layout=jp vt-switching=true
westonを再起動
$ systemctl restart weston@root
[launcher]とか追加すると多少は使い物になるかなと。
現状では上記設定にかかわらず最近の高解像度のモニターでは表示されないことが多いので、解像度はさほど高くなく複数入力の無いインテリでないものを使うことをお勧めする。
またOSが立ち上がる途中でHDMIケーブルを後差しすると表示されなかったり、後差ししないと表示されない等など人によって違うことを言われるが、調べがついていないので正直なところ良くわからない。
WiFiを使うのであればLinux Yocto User Manualの3.11に記載されている操作ではなく、ConnManを使うのが簡単。レシピではConnManのサービス化を止められていたので、まずは起動(以後定期的にスキャンが発動し、その都度シリアルポートにログを吐くようになるので注意)。
$ systemctl enable connman $ systemctl start connman
次にconnmanctlをインタラクティブモードで操作。
$ connmanctl connmanctl> enable wifi #WiFiをイネーブル Enabled wifi connmanctl> scan wifi #周辺のWiFiルータを検索 Scan completed for wifi connmanctl> services #検索結果を表示 .... * AR MyWiFiSSID wifi_abcdabcdabcd_aabbccddeeffaa_managed_psk .... connmanctl> agent on #エージェントを登録 Agent registered connmanctl> connect wifi_abcdabcdabcd_aabbccddeeffaa_managed_psk #検索結果のHASH値を使ってWiFiルータへつなぐ Agent RequestInput wifi_abcdabcdabcd_aabbccddeeffaa_managed_psk Passphrase = [ Type=psk, Requirement=mandatory, Alternates=[ WPS ] ] WPS = [ Type=wpspin, Requirement=alternate ] Passphrase? PASSPHRASE #WiFiルータの暗号化キーを入力 Connected wifi_abcdabcdabcd_aabbccddeeffaa_managed_psk connmanctl> quit #ひとまず終了
駆け足だがこれでつながっているので、ifconfig等で確認。
自動接続については、検索結果にあるWiFiルータのSSIDとHASH値を元に、/var/lib/connman/MyWiFiSSID-psk.configの名前で次のファイルを作成。MyWiFiSSID・HASH値・PASSPHRASEは適宜変更の事。
[service_wifi_abcdabcdabcd_aabbccddeeffaa_managed_psk] Type = wifi Name = MyWiFiSSID Passphrase = PASSPHRASE AutoConnect = True
RZBoardを再起動した際にIPアドレスが割り当てられていれば成功。WiFiに限らずBluetoothや有線LANにも使え、IPアドレスを固定させることも可。
Windowsが必要とされているのはeMMCへイメージを書き込む時以外に無いのだが、リモートクライアントとしてなら使い道はある。その際はsshクライアントのPuTTYを使えば便利。
リモートログインするなら
putty.exe -ssh -pw avnet root@rzboard:22
相互のファイル転送には同梱のpscpが使える。次のようなバッチを作っておくとコマンドライン上でのファイルコピーがお気楽(毎回ログインしに行くため遅い)。
@rem win to rzboard @pscp.exe -scp -unsafe -pw avnet %1 root@rzboard:%2
@rem rzboard to win @pscp.exe -scp -unsafe -pw avnet root@rzboard:%1 %2
もっと横着するならguiベースのsftpクライアントを使えば良い。以下の3つをインストールすればWindowsのドライブとしてrzboardをマウントできる。
マウントしたドライブ上のファイルを特定のツールで開こうとしてもドライブが見えなかったり、エディタ等で直接見に行くと時折待たされることがあるが、概ね許容範囲かと思う。
DHCPで割り当てられたRZBoardのIPアドレスが分からないからシリアルターミナルをつないでいるというのであれば、先の通り「rzboard」の名前で見つかる筈。逆にRZBoardからWindows PCや他のLinux PCの場合は「名前.local」で届く。
$ ping winpc.local -c 4 PING winpc.local (192.168.11.15): 56 data bytes 64 bytes from 192.168.11.15: seq=0 ttl=128 time=380.467 ms 64 bytes from 192.168.11.15: seq=1 ttl=128 time=148.134 ms 64 bytes from 192.168.11.15: seq=2 ttl=128 time=18.441 ms 64 bytes from 192.168.11.15: seq=3 ttl=128 time=41.408 ms --- winpc.local ping statistics --- 4 packets transmitted, 4 packets received, 0% packet loss round-trip min/avg/max = 18.441/147.112/380.467 ms $
元来DXHATを載せる必要は一切いが、RasPi互換を謳うなら多少なりとも使えるのでは?という事で評価を兼ねて。
HATの電源ボタンを押してもRZBoardの電源が入らない。PMICの詳細が公開されていないので不確かだが、ブロック図から推察してPMICにボタン入力が無いとレギュレータが生きず、DXHATのKILLが解放されないのだろう。
その後RZBoardのS1を押しながらHAT側の電源ボタンを押すと電源が入り、またS1を押しっぱなしでも電源が切れることは無い事がわかった。
S1の押下状態を維持するように短絡しておけば、HATの電源ボタンのみで電源供給のON/OFFは完結する。
なおDXHATの電源ボタンによるOSのシャットダウン励起は、憶測で色々試しすぎてRZBoardのGPIOが壊れてしまったようなのでペンディング。
RZBoard側にプルアップ抵抗が装備されていないようで、HAT上のI2CバスリピータのAサイドが正常に機能しない。
こちらも改造して良いのであれば、RZBoardのJ1-3(SDA1)とJ1-5(SCL1)をそれぞれ適当な値の抵抗を介してJ1-1(3V3)へ接続。
なおHATには電源バックアップされたRTCが載っているのだが、RZBoardのPMICにもRTCが装備されているようなのと、ドライバやレシピ云々の話になるのでひとまず放置。I2Cで直接アクセスして遊ぶ分には問題ない。
半二重I/FにSCIF2をつなぐには以下の箇所を半田ジャンパする。
Dynamixel Libraryについては、該当ポートを"/dev/ttySC2"に読み替えた上でDXHATのドキュメントの操作が一通り使える。
なおDynamixel Libraryのソースには日本語が含まれているので、先の日本語化対応しておかないと文字化けし、エディタで閲覧や編集が困難。
また通信速度を変えて試したところ、ボーレートジェネレータの分解能のせいか、1Mbps以上のボーレートは3M/1.5M/1Mbpsでのみ一致し、3Mbpsでは挙動が一部不審、3Mbpsを超えることはできなかった。
GPIOが活性化していれば回せる。
#!/bin/bash echo 193 > /sys/class/gpio/export echo out > /sys/class/gpio/P9_1/direction echo 1 > /sys/class/gpio/P9_1/value
しかしRaspberry Pi程の発熱がある訳でも無いのに、ビュービュー言わせながらファンを回し続ける意味は無い。以下のように温度を取得しそれに応じてファンを微風で回すプログラムをあてがえば十分。デーモンに登録して自動起動させれば良いだろう。
#include <stdio.h> #include <stdbool.h> #include <fcntl.h> #include <unistd.h> #include <signal.h> int fd_temp = -1; int fd_fan = -1; int fd_pb = -1; int fd_shut = -1; volatile sig_atomic_t term = 0; void handler (int sig, siginfo_t *info, void *ctx) { term = 1; } bool init (void) { int f, ff; // GPIOO9 (OUT) if ((f = open ("/sys/class/gpio/export", O_WRONLY)) == -1) { perror ("Unable to config GPIO9"); return false; } else { write (f, "193", 3); close (f); if ((ff = open ("/sys/class/gpio/P9_1/direction", O_WRONLY)) == -1) { perror ("Unable to config GPIO9"); return false; } else { write (ff, "out", 3); close (ff); if ((fd_fan = open ("/sys/class/gpio/P9_1/value", O_WRONLY)) == -1) { perror ("Unable to config GPIO9"); return false; } } } // GPIO48 (IN) if ((f = open ("/sys/class/gpio/export", O_WRONLY)) == -1) { perror ("Unable to config GPIO48"); return false; } else { write (f, "507", 3); close (f); if ((ff = open ("/sys/class/gpio/P48_3/direction", O_WRONLY)) == -1) { perror ("Unable to config GPIO48"); return false; } else { write (ff, "in", 2); close (ff); if ((fd_pb = open ("/sys/class/gpio/P48_3/value", O_RDONLY)) == -1) { perror ("Unable to config GPIO48"); return false; } } } // GPIO12 (OUT) if ((f = open ("/sys/class/gpio/export", O_WRONLY)) == -1) { perror ("Unable to open GPIO12"); return false; } else { write (f, "216", 3); close (f); if ((ff = open ("/sys/class/gpio/P12_0/direction", O_WRONLY)) == -1) { perror ("Unable to open GPIO12"); return false; } else { write (ff, "out", 3); close (ff); if ((fd_shut = open ("/sys/class/gpio/P12_0/value", O_WRONLY)) == -1) { perror ("Unable to open GPIO12"); return false; } } } // Temperatrue if ((fd_temp = open ("/sys/class/thermal/thermal_zone0/temp", O_RDONLY)) == -1) { perror ("Unable to open temp"); return false; } return true; } double get_temp (void) { char s[20]; lseek(fd_temp, 0, SEEK_SET); if (read (fd_temp, &s, sizeof(s)) > 0) { int t; sscanf (s, "%d", &t); fsync (fd_temp); return t / 1000.0; } return 999; } void set_fan (bool b) { write (fd_fan, b ? "1" : "0", 1); } bool get_pb (void) { char c; lseek(fd_pb, 0, SEEK_SET); read (fd_pb, &c, 1); return (c == '0'); } void set_shut (bool b) { write (fd_pb, b ? "1" : "0", 1); } int main (void) { double t; int cnt = 0, duty = 1; struct sigaction act; act.sa_sigaction = handler; act.sa_flags = SA_SIGINFO; if (sigaction(SIGINT, &act, NULL) < 0) { return 1; } act.sa_sigaction = handler; act.sa_flags = SA_SIGINFO; if (sigaction(SIGTERM, &act, NULL) < 0) { return 1; } if (init ()) { while (!term) { t = get_temp (); if (get_pb ()) duty = 10; else if (t > 55) duty = 10; else if (t > 50) duty = 6; else if (t > 45) duty = 4; else duty = 1; for (int i = 0; i < 10; i++) { set_fan (duty >= cnt); cnt++; if (cnt > 10) cnt = 0; usleep (5000); } } set_fan (0); close (fd_temp); close (fd_fan); close (fd_pb); close (fd_shut); return 0; } return 1; }
調子が良ければ概ね20秒程度でwestonの起動まで行きつく。ひとまずシンプルなアプリケーションがどの程度動くかを見るために、手持ちのRealSenseとシリアルサーボモータをつないで動かしてみたときの画面をキャプチャしてみた。
動画の最後、Power down後のpanicは横着してブートローダを更新しなかったからに他ならず。bitbakeした時に生成されたブートローダのイメージファイルをQSPIに書き込み、ようやく事なきを得る。ちなみにSDカードでブートする設定ではu-bootのsaveenvが失敗するため、環境変数やら設定等を覚えてくれることはない。
またブートローダ更新のついでにeMMCにもLinuxのイメージを書き込もうとしたところ、とてつもなく時間がかかる。ドキュメントにある操作中の画面のキャプチャでは114684KBの転送に15秒とあるので、その20倍以上かかる状況は何かおかしい。書き込み用のバッチを動かしていたPCをWiFiにつなげていた事が原因。有線に切り替える事で解消。
更にubuntuをホストPCとしているなら、シリアルターミナルのためだけにWindowsのTeraTermを引っ張り出してくる必要はない。そのPCにUSBシリアルI/Fをつないでターミナルにすれば良いまで。
$ screen /dev/ttyUSB0 115200,cs8
他に気づいたこととして、UARTから吐かれるログのタイミングからして極端に動作が遅い時がある。何に依存しているのか調べていないが、電源ON直後からDDRのクロック周波数が落ちている挙動に見受けられる。
J18/J20は使わないと勿体ない機能が含まれている。ヘッダは2-2367197-8なので、ハウジングの2-2367198-8を使って外線つなぐべし。
wget https://www.besttechnology.co.jp/download/DX2LIB_V3.0.zip unzip DX2LIB_V3.0.zip cd DX2LIB_v3.0/DX2LIB
¶{1,100,20}, // ID=1の100番地から20バイト分
Dynamixel Protocol 2 LibraryはDYNAMIXEL Communiation Protocol 2.0に対応した製品をWindows等のOSから操作するためのライブラリ集です。
シリアル通信に関するAPI、タイミングやエラー処理、プロトコルの整合性チェック等をライブラリ内で行うため、シリアル通信である事をほとんど意識する事無くアプリケーションの作りこみに専念できます。
なお、PCとBTE061D・BTE061E・BTE068・BTE068B・BTE068C・BTE082・BTE083・BTE074・BTE074B・BTE079・BTE080・BTE079B・BTE080B・BTE079C・BTE080C・BTE096・BTE101・BTE110・BTX229のいずれかがUSBポートに接続され、PCのOSに仮想COMポートが増設された状態で使用するものとします。BTE100の場合はRaspberry Piに依存します。
DYNAMIXEL Communiation Protocol 1.0とDYNAMIXEL Communiation Protocol 2.0を装備した装置を同一ネットワーク上で同時に運用する事は推奨できない。 |
以下のリンクよりライブラリ及びサンプルプログラムをアーカイブしたファイルがダウンロードできます。
最新版のアーカイブファイルには以下のファイルが同梱されます。必要に応じて解凍してください。
DX2LIB | dx2lib_x32.dll | ライブラリ本体 | |
dx2lib_x64.dll | |||
libdx2lib_x32.a | MINGW用ライブラリ(定義のみ) | ||
libdx2lib_x64.a | |||
dx2lib_x32.llb | MSVC用ライブラリ(定義のみ) | ||
dx2lib_x64.lib | |||
dx2lib.cpp | ライブラリソース | ||
dx2lib_intuitive.cpp | |||
dx2lib.h | ライブラリヘッダ | ||
dx2memmap.h | DXLアイテムアドレス定義ヘッダ | ||
dx2lib_matlab.h | matlab用ヘッダ | ||
dx2lib.py | python用モジュール | ||
makelib.cmd | Windows向けライブラリ再構築用バッチ | ||
build_dx2lib.sh | Linux向けライブラリ再構築用スクリプト | ||
SampleCode | C | smpl??.c | サンプル |
dxmisc.h | サンプル共通設定 | ||
kbhit.h | kbhitエミュレーション | ||
makefile | サンプルコンパイル用makefile | ||
makeexe.cmd | Windows向け再構築用バッチ | ||
dx2lib.h | DX2LIBフォルダに収録されるものと同一 | ||
dx2memmap.h | |||
dx2lib_x32.dll | |||
dx2lib_x64.dll | |||
DELPHI | Project1.dpr | サンプル | |
Unit1.dfm | |||
Unit1.pas | |||
DX2LIB.pas | |||
dx2lib_x32.dll | DX2LIBフォルダに収録されるものと同一 | ||
dx2lib_x64.dll | |||
Python | smpl??.py | サンプル | |
kbhit.py | kbhitエミュレーション | ||
dx2lib.py | DX2LIBフォルダに収録されるものと同一 | ||
dx2lib_x32.dll | |||
dx2lib_x64.dll | |||
LabVIEW2011 | sample??.vi | サンプル | |
DX2LIB.llb | DX2LIBラッパーライブラリ | ||
DX2LIB_DXL.llb | DX2LIBラッパーライブラリ | ||
DX2LIB_Wrapper.llb | |||
dx2lib_x32.dll | DX2LIBフォルダに収録されるものと同一 | ||
EXCEL | text.xls | サンプルシート | |
dx2lib_x32.dll | DX2LIBフォルダに収録されるものと同一 | ||
dx2lib_x64.dll | |||
Ruby | smpl??.rb | サンプル | |
dx2lib_x32.dll | DX2LIBフォルダに収録されるものと同一 | ||
dx2lib_x64.dll |
GCC Developer Liteの詳細についてはこちら。
「基本パック」と「WINパック」をダウンロードしてインストールしてください。
'SampleCodeC'フォルダにはAPIの基本的な使い方を紹介したサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
静的にDLLを使用する場合は以下の手順でDLLをリンクする指定を行った上でコンパイルします。なお32bit環境を前提とします。
ダウンロードファイルを解凍後、コンパイルするソースファイルと同一フォルダに以下のファイルをコピー
ファイル | ファイル名 | 備考 |
ヘッダ | dx2lib.h | 必要な宣言を集約 |
DLL | dx2lib_x32.dll | DLL本体(実行時およびリンク時に必要) |
ツールメニュー→コンパイラオプションをクリックし、表示されるダイアログボックスの設定リストから'x86 (Console)'を選択
etc...タブ内の追加ボタンを押し、新規に行を追加
新規に追加された空の行をクリックし'dx2lib_x32.dll'と入力
OKを押して設定を反映
動的にDLLを使用する場合はDLL自体をコンパイラオプションへ追記する必要はありません。その代わりソース中でdx2lib.hをインクルードする前に_DYNAMICLOADマクロを定義しておきます。
#define _DYNAMICLOAD #include "dx2lib.h"
これによりヘッダファイル内の諸定義が切り替わり、LoadDLLとUnloadDLLで動的なDLL読み込みと破棄ができます。
TDeviceID dev; // DLLを読み込み if (LoadDLL ()) { if ((dev = DX2_OpenPort (COMPORT, BAUDRATE))) { ... DX2_ClosePort (dev); } // DLLを破棄 UnloadDLL (); }
LoadDLLが成功していない状態で各APIを呼び出してはいけません。またDLL自体は実行プログラムと同じフォルダかOSがDLLを検索できる場所に配置しておく必要があります。
サンプルをコンパイルするだけならばGCC Developer Liteを起動する必要は無く、サンプルフォルダ内に同梱される「makeexe.cmd」を実行すればmakefileを読み込んでGCC Developer Liteに同梱されるmakeを使用してコンパイルします。
引数にソース名をしていするとそのソースだけコンパイル、何も付けなければフォルダ内の拡張子がCのソースファイル全てがコンパイル対象となります。
makeexe sample2_ping2 (特定のソースのみコンパイル) makeexe (カレントの全ソースをコンパイル)
なおI/Fのポート名やボーレート等はサンプル共通として「dxmisc.h」に記述してありますので、環境に応じて修正した上でコンパイルして下さい。
// マクロ定義 #if _WIN32 // Wiindows時 #define _COMPORT "\\.\COM3" // ポート名 #define _COMPORT2 "\\.\COM4" #else // LinuxやmacOS時 #define _COMPORT "/dev/ttyUSB0" // ポート名 #define _COMPORT2 "/dev/ttyUSB1" #endif #define _BAUDRATE (57600) // ボーレート[bps] #define _TARGETID (1) // 対象ID #define _TARGETID2 (2) // 対象ID
またバッチファイル内においてコンパイラなどのツール類へのパスを通す際に64bit環境を優先しています。
Visual C++ 2010 ExpressでDLLの静的なリンク方法のみ紹介します。
ダウンロードファイルを解凍後、コンパイルするソースファイルと同一フォルダに以下のファイルをコピー
ファイル | ファイル名 | 備考 |
ヘッダ | dx2lib.h | 必要な宣言を集約 |
DLL | dx2lib_x32.dll | 実行時に必要 |
ライブラリ | dx2lib_x32.lib | リンク時に必要 |
構成プロパティ→C/C++→全般→追加のインクルードディレクトリの項目にヘッダファイルの格納フォルダを指定
構成プロパティ→リンカー→全般→追加のライブラリディレクトリの項目にLIBファイルの格納フォルダを指定
構成プロパティ→リンカー→入力→追加の依存ファイルの項目にLIBファイル名を指定
構成プロパティ→C/C++→プリコンパイル済ヘッダーの項目にプリコンパイル済ヘッダーを使用しないを指定
'SampleCodeLabVIEW2011'フォルダにサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
LabVIEWには外部のDLLへアクセスする手段が提供されています。しかしながらDX2LIBそのままの定義ではLabVIEWからは使いづらいため、サンプルプログラムではサブviでラッピングしてDX2LIB.llbにまとめて提供しています。
'SampleCodeDELPHI'フォルダにサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
DELPHIはPASCAL言語によるPC向け開発ツールであり、外部のDLLへ容易にアクセスする事ができます。サンプルに含まれるDX2LIB.pas内にdx2lib_x32.dllないしdx2lib_x64.dllを動的にロードする関数を用意しましたので、ユーザソースのuses節にdx2libを追記すればDynamixel Libraryの各APIへ簡便にアクセスできます。
'SampleCodeEXCEL'フォルダにサンプルが同梱されます。
ここではVBと似たMicrosoft OfficeのVBAを使用し、マクロの標準モジュールにDX2LIBHEADという名称でDLLに含まれるいくつかのAPIが定義してあります。
Module1にPingTestとMotorTestというマクロが記述されていますので、ワークシートから適宜マクロを呼び出して実行してください。結果がシート上に反映されます。
'SampleCodeRuby'フォルダにサンプルが同梱されます。ライブラリ名・ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
Rubyはオープンソースの動的なプログラミング言語で、外部のライブラリはfiddleを使用してアクセスします。
require "fiddle/import" module DX2LIB extend Fiddle::Importer dlload './dx2lib_x32.dll' extern 'void * DX2_OpenPort( const char *, long )' extern 'char DX2_Active( void * )' extern 'char DX2_Ping( void *, unsigned char, unsigned short * )' extern 'char DX2_ClosePort( void * )' end devid = dx2lib.DX2_OpenPort( 'ポート名', ボーレート )
LinuxやmacOSの場合は予めライブラリを再構築しておいて下さい。
'SampleCodePython'フォルダにサンプルが同梱されます。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して使用します。
Pythonはオープンソースの動的なプログラミング言語で、外部のDLLへ簡易にアクセスできます。
各APIのPython用の宣言を定義したdx2lib.pyをimportするだけで済みます。
from dx2lib import * # dx2libをインポート
なおdx2lib.pyはctypesによるDLLの単純なラッパーに過ぎませんので、Pythonならではの抽象的な定義は何一つ受け付けてくれません。Pythonからプログラミングを始めた方は微妙に扱いづらいかと思いますので、詳細はサンプルコードを参考にしてください。
LinuxやmacOSの場合は予めライブラリを再構築しておいて下さい。
MATLABからの使用例を紹介します。ポート・ボーレート・ID等は使用する環境に合わせて適宜修正して下さい。また、mex-setupにてCコンパイラを選択しておく必要があります。
まずはMATLAB起動後、「ファイル(F)」→「パス設定(H)」でdx2lib_x32.dll(MATALBが64bitの場合はdx2lib_x64.dll)とdx2lib_matlab.hの格納されたパスを指定します。dx2lib.hはMATLABでは解釈できない記述が多いため使用しないでください。
loadlibrary('dx2lib_x32.dll','dx2lib_matlab.h','alias','dx2lib')
ロードされたdx2libのDX2_OpenPortを呼出します。関数名の後はポートとボーレートです。
devid = calllib('dx2lib','DX2_OpenPort','\.COM3',1000000)
TErrorCodeはポインタで引き渡しているため、事前に型宣言をしておきます。値はErr.Valueで取得可能です。必要なければ0を指定しても構いません。
Err = libpointer('uint16Ptr', 0); Ret = calllib('dx2lib', 'DX2_Ping', devid, 1, Err); fprintf('%x', Err.Value);
DX2_OpenPortを行ったので、DX2_ClosePortを使用してポートを閉じます。
calllib('dx2lib','DX2_ClosePort',devid)
unloadlibrary('dx2lib')
LinuxないしmacOS上でのdx2libのコンパイル方法を紹介します。
wget https://www.besttechnology.co.jp/download/DX2LIB_V3.0.zip unzip DX2LIB_V3.0.zip
cd DX2LIB_v3.0/DX2LIB bash ./build_dx2lib.shコンパイルが成功するとライブラリファイルがサンプルディレクトリにコピーされる
gcc sample.c -L. -ldx2lib -o sample
'SampleCodeC'のサンプルプログラムはmakeを使ってコンパイルできます。
make sample2_ping2 (特定のソースのみコンパイル) make (カレントの全ソースをコンパイル)
コンパイルや実行にあたってI/Fやカーネル・ディストリビューションに依存しますので、そのまま使用できない場合は適宜ソースを修正下さい。またWindowsを前提とした_DYNAMICLOADマクロが宣言されているとコンパイルできません。
なおI/Fのポート名やボーレート等はサンプル共通として「dxmisc.h」に記述してありますので、環境に応じて修正した上でコンパイルして下さい。
またmacOSの場合のポート名は「/dev/tty.usbserial-????」ではなく「/dev/cu.usbserial-????」を指定してください。更に最近のmacOSにおける標準の機能ではlatency timeを変更できなかったので、以下のようなコードで対応してみてください(USB I/Fの抜き挿しの度に実行する必要がありますが...)。
// brew install libftdi // cc -I/opt/homebrew/include/libftdi1 -L/opt/homebrew/lib -lftdi1 setlatency.c -o setlatency #include <stdio.h> #include <stdlib.h> #include <stdbool.h> #include <ftdi.h> int main (void) { int result = EXIT_FAILURE; struct ftdi_context *ftdic; struct ftdi_device_list *devlist; bool f = true, b = false; if ((ftdic = ftdi_new ()) != 0) { ftdi_set_interface (ftdic, INTERFACE_ANY); int num = ftdi_usb_find_all (ftdic, &devlist, 0, 0); if (num > 0) { for (int i = 0; i < num; i++) { printf ("%d: ",i); if (ftdi_usb_open_dev (ftdic, devlist[i].dev) == 0) { char s[2][512]; unsigned char prev_latency; if (ftdi_read_eeprom(ftdic) == 0 && ftdi_eeprom_decode(ftdic,0) == 0 && ftdi_eeprom_get_strings(ftdic, s[0], 512, s[1], 512, NULL, 0) == 0) printf("[%s][%s] ", s[0], s[1]); if (ftdi_get_latency_timer (ftdic, &prev_latency) == 0) { b = (ftdi_set_latency_timer (ftdic, 1) == 0); printf ("prev latency = %i to %sn", prev_latency, b ? "1" : "?"); } else b = false; f = f && b; } ftdi_usb_close (ftdic); } } else printf ("no device found.n"); if (f && b) result = EXIT_SUCCESS; ftdi_list_free (&devlist); ftdi_free (ftdic); } return result; }
Dynamixel Libraryではシリアル通信を直接意識するコードを記述せずに、対象IDのデバイスのコントロールテーブルへの読み書き行うAPIを用意しています。
C言語のソースにdx2lib.hをインクルードすれば、APIを使用するのに必要なプロトタイプとマクロの定義がなされます。
注意事項:
DynamixelのStatus Return Levelを必ず2に設定した上で使用する事。それ以外の値が設定されていた場合はAPIが想定した応答が得られず、タイムアウトするまでAPIから返らない。
ライブラリの内部情報を初期化すると同時に指定されたCOMポートをオープンし、DX2_SetBaudrateを使用して通信速度を設定した後、ユニークなTDeviceIDを返す。以後はこのTDeviceIDを使用して各APIを使用する。
複数のCOMポートを使用する場合は使用するポート毎にDX2_OpenPortを行い、各々のポートに対してTDeviceIDを取得しなくてはならない。
なお、Linuxにおけるボーレートの指定に関しては、DX2_SetBaudrateの解説に注意の事。
TDeviceID DX2_OpenPort (char *name, uint32_t baud);
インターフェースが提供するCOMポート名。
Windowsの場合の記述方法はこちらの情報に従う。
インターフェースとデバイス間の通信速度[bps]。
オープンに成功した場合は0以外の値、失敗した場合は0を返す。
TDeviceID dev; // COM10を9600bpsでオープン dev = DX2_OpenPort ("\\.\COM10", 9600);
DX2_OpenPortで開いたCOMポートを閉じる。
DX2_ClosePortが実行された以後はTDeviceIDは使用できなくなる。
bool DX2_ClosePort (TDeviceID dvid);
DX2_OpenPortで開いた際のTDeviceID。
クローズに成功した場合はtrue、失敗した場合はfalseを返す。
TDeviceID dev; // オープン dev = DX2_OpenPort ("\\.\COM10", 9600); if (dev) { ... (中略) // クローズ DX2_ClosePort (dev); }
既にオープンされているTDeviceIDの通信速度の変更を行う。
実行すると強制的に受信バッファがクリアされる。
bool DX2_SetBaudrate (TDeviceID dvid, long baud);
DX2_OpenPortで開いた際のTDeviceID。
新しい通信速度[bps]。
通信速度の変更が成功するとtrue、失敗するとfalseを返す。
TDeviceID dev; // オープン dev = DX2_OpenPort ("\\.\COM10", 9600); if (dev) { // 通信速度を1M[bps]に変更 DX2_SetBaudrate (dev, 1000000); ... (中略) // クローズ DX2_ClosePort (dev); }
指定されたTDeviceIDのポートが開かれており、使用可能であるかを確認する。
USB接続等によりインターフェース自体が取り外し可能な場合に、実際に使用可能であるかを判断するために使用するが、状況によっては正確に判断できない場合もある。
bool DX2_Active (TDeviceID dvid);
DX2_OpenPortで開いた際のTDeviceID。
指定されたdvidが使用可能な場合はtrue、使用不可の場合はfalseを返す。
TDeviceID dev; // オープン dev = DX2_OpenPort ("\\.\COM10", 9600); if (dev) { while (DX2_Active (dev)) { ... (中略) } // クローズ DX2_ClosePort (dev); }
I/FやOSの都合で生じるであろうタイムラグを予め設定する。
内部で算出している受信タイムアウト時間とタイムアウトオフセット時間を加算した時間を超えた場合に、タイムアウトエラーとして処理する。
デフォルトは30[ms]。
void DX2_SetTimeOutOffset (TDeviceID dvid, uint32_t offsettime);
DX2_OpenPortで開いた際のTDeviceID。
タイムアウトオフセット時間[ms]
指定されたdvidが使用可能な場合はtrue、使用不可の場合はfalseを返す。
PINGインストラクションを使用して対象IDからの応答を確認する。
bool DX2_Ping (TDeviceID dvid, uint8_t id, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252, 254)。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; // オープン dev = DX2_OpenPort ("\\.\COM10", 57143); if (dev) { // ID=1にPINGを発行 if (DX2_Ping (dev, 1, &err)) printf ("Found [%08X]n", err); else printf ("Not found [%08X]n", err); // クローズ DX2_ClosePort (dev); }
PINGインストラクションでBROADCASTING IDを指定して不特定の対象の応答を確認する。
BROADCASTING IDを指定した場合の応答時間はデバイスによって差があるため、異なるデバイスが混在している環境では正確な情報を取得できない場合がある。
bool DX2_Ping2 (TDeviceID dvid, uint32_t *num, TDxAlarmStatus *AlarmStatus, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
検索する最大数及び検索で見つかったデバイス数の保存先。
検索で見つかったデバイス情報の保存先。
少なくともnumで指定したサイズ分の領域を確保しておく必要がある。
エラーコード。
1台以上のデバイスからの応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; uint8_t id; TDxAlarmStatus stat[254]; int i; uint32_t num = 252; // オープン dev = DX2_OpenPort ("\\.\COM10", 57143); if (dev) { // 不明な対象に対してPINGを発行 if (DX2_Ping2 (dev, &num, stat, &err)) { for (i = 0; i < num; i++) printf ("Found ID=%d %02Xn", stat[i].id, stat[i].Status); } // クローズ DX2_ClosePort (dev); }
RESETインストラクションを使用して対象IDを出荷時の状態に初期化する。
bool DX2_Reset (TDeviceID dvid, uint8_t id, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252, 254)。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; // オープン dev = DX2_OpenPort ("\\.\COM10", 57143); if (dev) { // ID=1にRESETを発行 if (DX2_Reset (dev, 1, &err)) printf ("Success [%08X]n", err); else printf ("Fail [%08X]n", err); // クローズ DX2_ClosePort (dev); }
REBOOTインストラクションを使用して対象IDを再起動する。
bool DX2_Reboot (TDeviceID dvid, uint8_t id, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252, 254)。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; // オープン dev = DX2_OpenPort ("\\.\COM10", 57143); if (dev) { // ID=1にREBOOTを発行 if (DX2_Reboot (dev, 1, &err)) printf ("Success [%08X]n", err); else printf ("Fail [%08X]n", err); // クローズ DX2_ClosePort (dev); }
対象IDのコントロールテーブルから1バイトのデータを読み出す。
bool DX2_ReadByteData(TDeviceID dvid, uint8_t id, uint16_t adr, uint8_t *rdata, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
コントロールテーブルのアドレス。
読み出した値の保存先。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; uint8_t dat; // オープン dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXL-320からLEDの状態を取得 if (DX2_ReadByteData (dev, 1, 25, &dat, &err)) { printf ("LED STAT=%dn", dat); } DX2_ClosePort (dev); }
対象IDのコントロールテーブルへ1バイトのデータを書き込む。
bool DX2_WriteByteData(TDeviceID dvid, uint8_t id, uint16_t adr, uint8_t dat, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252, 254)。
コントロールテーブルのアドレス。
書き込む値。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
BROADCASTING IDを指定した場合は応答待ちを行わない。
TDeviceID dev; TErrorCode err; uint8_t dat; // オープン dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXL-320からLEDの状態を取得 if (DX2_ReadByteData (dev, 1, 25, &dat, &err)) { dat = (dat + 1) & 7; // ID=1のXL-320へLEDの状態を設定 DX2_WriteByteData (dev, 1, 25, dat, &err); } DX2_ClosePort (dev); }
対象IDのコントロールテーブルから1ワード(2バイト)のデータを読み出す。
bool DX2_ReadWordData(TDeviceID dvid, uint8_t id, uint16_t adr, uint16_t *rdata, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
コントロールテーブルのアドレス。
読み出した値の保存先。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; uint16_t dat; // オープン dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXL-320から現在位置を取得 if (DX2_ReadWordData (dev, 1, 30, &dat, &err)) { printf ("PRESENT POS=%dn", dat); } DX2_ClosePort (dev); }
対象IDのコントロールテーブルへ1ワード(2バイト)のデータを書き込む。
bool DX2_WriteWordData(TDeviceID dvid, uint8_t id, uint16_t adr, uint16_t dat, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252, 254)。
コントロールテーブルのアドレス。
書き込む値。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
BROADCASTING IDを指定した場合は応答待ちを行わない。
TDeviceID dev; TErrorCode err; // オープン dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXL-320へ位置(511)を指令 DX2_WriteWordData (dev, 1, 30, 511, &err); DX2_ClosePort (dev); }
対象IDのコントロールテーブルからロング(4バイト)のデータを読み出す。
bool DX2_ReadLongData(TDeviceID dvid, uint8_t id, uint16_t adr, uint32_t *rdata, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
コントロールテーブルのアドレス。
読み出した値の保存先。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; uint32_t dat; // オープン dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXM430から現在位置を取得 if (DX2_ReadWordData (dev, 1, 132, &dat, &err)) { printf ("PRESENT POS=%dn", dat); } DX2_ClosePort (dev); }
対象IDのコントロールテーブルへロング(4バイト)のデータを書き込む。
bool DX2_WriteLongData(TDeviceID dvid, uint8_t id, uint16_t adr, uint32_t dat, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252, 254)。
コントロールテーブルのアドレス。
書き込む値。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
BROADCASTING IDを指定した場合は応答待ちを行わない。
TDeviceID dev; TErrorCode err; // オープン dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXM430へ位置(2047)を指令 DX2_WriteWordData (dev, 1, 116, 2047, &err); DX2_ClosePort (dev); }
対象IDのコントロールテーブルから指定サイズのデータを読み出す。
bool DX2_ReadBlockData (TDeviceID dvid, uint8_t id, uint16_t adr, uint8_t *rdata, uint32_t len, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
コントロールテーブルのアドレス。
読み出したデータの保存先。
読み出すデータのサイズ。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; uint8_t gain[3]; dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXL-320からPIDゲインのデータを取得 if (DX2_ReadBlockData (dev, 1, 26, gain, 3, &err) { printf ( "D=%d I=%d P=%dn", gain[0], gain[1], gain[2] ); } DX2_ClosePort (dev); }
対象IDのコントロールテーブルへ指定サイズのデータを書き込む。
bool DX2_WriteBlockData(TDeviceID dvid, uint8_t id, uint16_t adr, const uint8_t *dat, uint32_t len, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252, 254)。
コントロールテーブルのアドレス。
書き込むデータの保存先。
書き込むデータのサイズ。
エラーコード。
正常な応答が得られた場合はtrue、それ以外はfalseを返す。
BROADCASTING IDを指定した場合は応答待ちを行わない。
TDeviceID dev; TErrorCode err; uint8_t gain[3] = { 1, 1, 1 }; dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のXL-320のPIDゲインを変更 DX2_WriteBlockData (dev, 1, 26, gain, 3, &err); DX2_ClosePort (dev); }
SYNC READインストラクションを使用して複数IDからブロック読み出しを行う。
読み出されるデータの1軸分の構造は[ID(8bit)][ERR(16bit)][DATA1(8bit)]...[DATAn(8bit)]となり、複数軸の場合は本データが軸数分だけスタックする。
bool DX2_ReadSyncData (TDeviceID dvid, const TSyncReadParam *param, uint32_t *num, uint8_t *dat, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
パラメータデータを指定。
読み出すデバイス数を指定。APIの処理後に応答したデバイス数が入る
取得データの保存先。((受信データ長 + 3) * num)バイト以上を確保の事。
エラーコード。
指定したデバイス数が応答したらtrue、それ以外はfalseを返す。
// XM430の120番地から2バイト分のデータをid=1,2,3より読み出すパラメータ const TSyncReadParam param = { addr:120, length:2, ids:{1,2,3}, }; // 読み出しすデータの構造体 typedef struct { uint8_t id; TErrorCode err; uint16_t RTTick; } __attribute__ ((gcc_struct, __packed__)) TSyncR[3]; TSyncR rdat; TDeviceID dev; TErrorCode err; if ((dev = DX2_OpenPort ("\\.\COM5", 1000000))) { uint32_t num = 3; // 3軸 DX2_ReadSyncData (dev, ¶m, &num, (uint8_t *)&rdat, &err); printf("resul num=%dn", num); for (int i = 0; i < num; i++) { printf("ID:%d ERR:%04X tick=%dn", rdat[i].id, rdat[i].err, rdat[i].RTTick); } DX2_ClosePort (dev); }
SYNCインストラクションを使用して複数IDへブロック書き込みを行う。
書き込まれるデータの構成はユーザに委ねられる。
bool DX2_WriteSyncData (TDeviceID dvid, uint8_t *dat, uint32_t size, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
書き込むパラメータの保存先。
パラメータのサイズ。
エラーコード。
インターフェースより送信が行われた場合はtrue、それ以外はfalseを返す。
// 2台のXM430へ位置を指令する想定の構造体 typedef struct { uint16_t addr; uint16_t length; struct { uint8_t id; uint32_t goalpos; } __attribute__ ((gcc_struct, __packed__)) data[2]; } __attribute__ ((gcc_struct, __packed__)) TSyncW; TSyncW syncw = { 116, // アドレス (Goal Position) 4, // データ長 (4 byte) {{ 1, 3072 }, // 1軸目 ID=1, GoalPosition=3072 { 2, 1024 }} // 2軸目 ID=2, GoalPosition=1024 }; TDeviceID dev; TErrorCode err; dev = DX2_OpenPort ("\\.\COM5", 1000000); if (dev) { // 2台のXM430へ個別の位置を指令 DX2_WriteSyncData (dev, (uint8_t *)&syncw, sizeof(syncw), &err); DX2_ClosePort (dev); }
BULK READインストラクションを使用して複数IDの指定アドレス・サイズでブロック読み出しを行う。
読み出されるデータの1軸分の構造は[SIZE(16bit)][ID(8bit)][ERR(16bit)][DATA1(8bit)]...[DATAn(8bit)]とし、DATAの前にSIZE(SIZEを含む1軸分のデータ長)・ID・ERRの計5バイトが付与される。複数軸の場合はSIZEから始まる本データが軸数分連続して取得される。
bool DX2_ReadBulkData (TDeviceID dvid, const TBulkReadParam *param, uint32_t *num, uint8_t *dat, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
パラメータデータを指定。
読み出すデバイス数を指定。APIの処理後に応答したデバイス数が入る
取得データの保存先。十分な領域を確保の事。
TBulkReadResultを介して解析すると便利。
エラーコード。
指定したデバイス数が応答したらtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; uint8_t rdat[1000]; // 読み出したデータの保存先 const TBulkReadParam param[3] = { {1,100,20}, // ID=1の100番地から20バイト分 {2,124,12}, // ID=2の124番地から12バイト分 {3, 0, 7} // ID=3の0番地から7バイト分 }; dev = DX2_OpenPort ("\\.\COM5", 1000000); if (dev) { uint32_t num = 3; // 3軸 PBulkReadResult pr = (void *)rdat; DX2_ReadBulkData (dev, ¶m[0], &num, (uint8_t *)&rdat, &err); printf("resul num=%d err=$%04xn", num, err); for (int i = 0; i < num; i++) { if (pr->size > 0) { printf("nID:%d err:$%04X ",pr->id, pr->err); for (int j = 0; j < pr->size - 5; j++) printf("%02X ", pr->dat[j]); pr = (void *)pr + pr->size; // 次のデータ用にポインタを更新 } } DX2_ClosePort (dev); }
BULKインストラクションを使用して複数の個別なID・アドレス・データサイズで書き込みを行う。
bool DX2_WriteBulkData (TDeviceID dvid, uint8_t *dat, uint32_t size, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
書き込むパラメータの保存先。
パラメータのサイズ。
エラーコード。
インターフェースより送信が行われた場合はtrue、それ以外はfalseを返す。
// 3台のXM430へ位置を指令する想定の構造体 typedef struct { uint8_t id; uint16_t addr; uint16_t len; int32_t pos; } __attribute__ ((gcc_struct, __packed__)) TBW[3]; TBW BW = { {1, 116, 4, 0}, // 1軸目 ID=1, アドレス=116, サイズ=4, GPos=0 {2, 116, 4, 2047}, // 2軸目 ID=1, アドレス=116, サイズ=4, GPos=2047 {3, 116, 4, 4095} // 3軸目 ID=1, アドレス=116, サイズ=4, GPos=4095 }; TDeviceID dev; TErrorCode err; dev = DX2_OpenPort ("\\.\COM5", 1000000); if (dev) { // 3台のXM430へ個別の情報を書き込み DX2_WriteBulkData (dev, (uint8_t *)&BW, sizeof(BW), &err); DX2_ClosePort (dev); }
任意のインストラクションパケットを送信する。
bool DX2_TxPacket (TDeviceID dvid, uint8_t id, TInstruction inst, uint8_t *param, uint32_t len, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
使用するインストラクション。
送信するパラメータの保存先。
送信するパラメータのサイズ。
エラーコード。
インターフェースより送信が行われた場合はtrue、それ以外はfalseを返す。
TDeviceID dev; TErrorCode err; uint8_t param[2] = { 25, // アドレス (LED) 0, // データ }; dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のAX-12+のLEDを消灯 DX2_TxPacket (dev, 1, INST_WRITE, param, 2, &err); DX2_ClosePort (dev); }
ステータスパケットを受信する。
基本的にDX2_TxPacketとペアで使用する。ステータスパケットが得られない状況で使用するとタイムアウトするまで返らない。
なお、本APIはDX2_SetTimeOutOffsetで設定されたオフセット値は使用せず、引数で指定された受信タイムアウトのみが適用される。
bool DX2_RxPacket (TDeviceID dvid, uint8_t *rdata, uint32_t rdatasize, uint32_t *rlen, uint32_t timeout, TErrorCode *err);
DX2_OpenPortで開いた際のTDeviceID。
受信バッファ。
ステータスパケットを受信するのに十分なサイズを確保しておく必要がある。
rdataのサイズ。
実際に受信されたステータスパケットのサイズ。
受信タイムアウト[ms]。
エラーコード。
受信成功時はtrue、それ以外はfalseを返す。
int i; uint32_t len; TDeviceID dev; TErrorCode err; uint8_t param[2] = { 25, // アドレス (LED) 1, // サイズ }; dev = DX2_OpenPort ("\\.\COM10", 1000000); if (dev) { // ID=1のAX-12+からLEDの状態を読み出す要求 if (DX2_TxPacket (dev, 1, INST_READ, param, 2, &err)) { // ステータスパケットを受信 DX2_RxPacket (dev, dat, sizeof (dat), &len, 100, &err); for (i = 0; i < len; i++) { printf ("[%02X]", dat[i]); } } DX2_ClosePort (dev); }
INST_PING
INST_READ
INST_WRITE
INST_REG_WRITE
INST_ACTION
INST_RESET
INST_REBOOT
INST_STATUS
INST_SYNC_READ
INST_SYNC_WRITE
INST_BULK_READ
INST_BULK_WRITE
bit | macro name | |
15 | ERR_INVALID_DEVID | 使用できないTDeviceID |
14 | ERR_INVALID_ID | 指定できないID |
13 | ERR_DIFF_ID | 異なるIDからの応答 |
12 | ERR_ILLEGAL_SIZE | 異常なデータサイズ |
11 | ERR_INVALID_PARAM | 異常なパラメータ |
10 | ERR_COMM | シリアルポートエラー |
9 | ERR_CHECKSUM | 異常なチェックサム |
8 | ERR_TIMEOUT | 受信タイムアウト |
7 | ERR_DX2_ALERT | ハード的な異常検出 |
6 | ERR_DX2_ACCESS=7 ERR_DX2_DATALIMIT=6 ERR_DX2_DATALENGTH=5 ERR_DX2_DATARANGE=4 ERR_DX2_CRC=3 ERR_DX2_INSTRUCTION=2 ERR_DX2_RESULT=1 | パケットの処理や数値範囲に関するエラー |
5 | ||
4 | ||
3 | ||
2 | ||
1 | ||
0 |
使用するDynamixelの情報を予めライブラリ内に保持させておき、位置や角速度等の指令およびフィードバック値は物理値を扱い、各々の区別はIDのみで行うAPIです。
これAPIによりDynamixelのモデルごとに異なるコントロールテーブルや煩雑な運転方法の違いを意識する事なく、少ないコードで目的の挙動を実現できます。
なお、従来通りコントロールテーブルへの直接アクセスを制限するものではありませんが、その場合は本機能が想定する状況と一致しなくなる事があります。
※以下の使用例はDX2LIBを前提としています。
Dynamixelが持つ固有の情報を予め記憶させておくことで、それ以後の操作はIDのみを指定し、どのモデルであっても使用頻度が高いアイテムはコントロールテーブルを意識する事無く同じAPIで同様の動作ができます。
ID0~252のデバイスを順次検索し、サポートするDynamixelであればライブラリ内のリストに登録する。
リストに登録されていないデバイスのIDに対しては存在しないものとして通信を行わないため、DXL_ScanDevicesかDXL_GetModelInfoを用いて予めリストに登録しておく必要がある。
int DXL_ScanDevices (TDeviceID dvid, uint8_t *ids);
DX?_OpenPortで開いた際のTDeviceID。
見つかったデバイスのID一覧を必要に応じて取得する配列の保存先。最小でも253バイトを確保すること。
見つかったデバイスの数。
#include <stdio.h> #include <dx2lib.h> void main (void) { // COM10を57600bpsでオープン TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); if (dev != 0) { // ID=0~252の情報を取得し成功したもののみライブラリ内のリストに追加 printf("detect num=%dn", DXL_ScanDevices (dev, NULL)); DX2_Close (dev); } }
指定IDのモデル情報の取得し、サポートするDynamixelであればライブラリ内のリストに登録する。
リストに登録されていないデバイスのIDに対しては存在しないものとして通信を行わないため、DXL_ScanDevicesかDXL_GetModelInfoを用いて予めリストに登録しておく必要がある。
PDXL_ModelInfo DXL_GetModelInfo (TDeviceID dvid, uint8_t id);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
登録されたIDと一致するTDXL_ModelInfoのアドレス。
#include <stdio.h> #include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("/dev/ttyUSB0", 57600); for (uint8_t id = 0; id <= 252; id++) { // idの情報を取得し成功したらライブラリ内のリストに追加 PDXL_ModelInfo p = DXL_GetModelInfo (dev, id); if (p->modelno != 0) printf ("[%3d] %s ($%04X) %dn", id, p->name, p->modelno, p->devtype); } } DX2_ClosePort (dev); }
ライブラリ内のリストに登録された全てのデバイスの情報をコンソールに出力する。
ID・モデル名・モデル番号の順に出力される。
bool DXL_PrintDevicesList (int (*pf) (const char *, ...));
printf等の書式化文字列出力ルーチンのポインタ。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <stdio.h> #include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("/dev/ttyUSB0", 57600); DXL_ScanDevices (dev, NULL); DXL_PrintDevicesList ((void *)&printf); DX2_ClosePort (dev); }
追加APIの処理の中で最後に取得したエラーを返す。本API自体は通信を行わない。
TErrorCode DXL_GetErrorCode (TDeviceID dvid, uint8_t id);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
エラーコード。
対象IDのコントロールテーブルにハードウェアエラーが備わっている場合にのみ取得する。
bool DXL_GetHWErrorCode (TDeviceID dvid, uint8_t id, uint8_t *hwerr);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
取得されたハードウェアエラーの保存先。
処理が正常終了したらtrue、それ以外はfalseを返す。
運転モードはDynamixelのDrive Mode、動作モードはOperating Modeを意味します。
運転モードはデフォルトの回転方向やプロファイルの選択、マスタースレーブの設定を行います。
動作モードについてはDynamixel X・Pシリーズの動作モードを基準とし、他のモデルにおいてもこの動作モードの番号を踏襲することとします。
対象IDの運転モードを変更する。
指定されたモードと現在のモードが異なる場合にのみトルクをOFFにし運転モードを更新する。
対称IDがライブラリ内のリストに登録されている必要がある。
bool DXL_SetDriveMode (TDeviceID dvid, uint8_t id, uint8_t mode);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
機種依存あり
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); // ID:1を検索し見つかれば登録 DXL_GetModelInfo (dev, 1); // ID:1のXシリーズをTime-Base Profileに設定 DXL_SetDriveMode (dev, 1, 4); DX2_ClosePort (dev); }
複数IDの運転モードを一括変更する。
指定されたモードと現在のモードが異なる場合にのみトルクをOFFにし運転モードを更新する。
ライブラリ内のリストに登録されていないIDは無視される。
bool DXL_SetDriveModesEquival (TDeviceID dvid, const uint8_t *ids, int num, uint8_t mode);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
対象とするID数
機種依存あり
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); uint8_t ids[3] = {1, 2, 3}; // ID:1~3を検索し見つかれば登録 for (int i = 0; i < sizeof (ids); i++) DXL_GetModelInfo (dev, ids[i]); // ID:1~3のDrive Modeを出荷時デフォルト値に設定 DXL_SetDriveModesEquival (dev, ids, sizeof (ids), 0); DX2_ClosePort (dev); }
対象IDの動作モードを変更する。
指定されたモードと現在のモードが異なる場合にのみトルクをOFFにし動作モードを更新する。
対称IDがライブラリ内のリストに登録されている必要がある。
bool DXL_SetOperatingMode (TDeviceID dvid, uint8_t id, uint8_t mode);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
Dynamixel Xシリーズの動作モードを想定し、それに完全対応しないデバイスに対しては近似値もしくは失敗を返す。
0: Current Control (電流制御)
1: Velocity Control (速度制御)
3: Position Control (位置制御)
4: Expand Position Control (多回転位置制御)
5: Current-Base Position Control
16: PWM Control
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); // ID:1を検索し見つかれば登録 DXL_GetModelInfo (dev, 1); // ID:1をCurrent-Base Position Controlに設定 DXL_SetOperatingMode (dev, 1, 5); DX2_ClosePort (dev); }
複数IDの動作モードを一括変更する。
指定されたモードと現在のモードが異なる場合にのみトルクをOFFにし動作モードを更新する。
ライブラリ内のリストに登録されていないIDは無視される。
bool DXL_SetOperatingModesEquival (TDeviceID dvid, const uint8_t *ids, int num, uint8_t mode);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
対象とするID数
Dynamixel Xシリーズの動作モードを想定し、それに完全対応しないデバイスに対しては近似値もしくは失敗を返す。
0: Current Control
1: Velocity Control
3: Position Control
4: Expand Position Control
5: Current-Base Position Control
16: PWM Control
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); uint8_t ids[3] = {1, 2, 3}; // ID:1~3を検索し見つかれば登録 for (int i = 0; i < sizeof (ids); i++) DXL_GetModelInfo (dev, ids[i]); // ID:1~3をVelocity Controlに設定 DXL_SetOperatingModesEquival (dev, ids, sizeof (ids), 1); DX2_ClosePort (dev); }
対象IDの動作モードを取得する。なお内部ではDrive Modeも一緒に取得し保持している。
bool DXL_GetOperatingMode (TDeviceID dvid, uint8_t id, uint8_t *mode);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
取得した動作モードの保存先。
処理が正常終了したらtrue、それ以外はfalseを返す。
対象IDのLEDを明滅させる。
bool DXL_SetLED (TDeviceID dvid, uint8_t id, bool en);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
trueで点灯、falseで消灯。
処理が正常終了したらtrue、それ以外はfalseを返す。
対象IDの制御を開始ないし停止させる。
なお、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetTorqueEnable (TDeviceID dvid, uint8_t id, bool en);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
trueで開始、falseで停止。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); DXL_GetModelInfo (dev, 1); DXL_SetOperatingMode (dev, 1, 3); // ID:1の制御をON DXL_SetTorqueEnable (dev, 1, true); // ID:1へ角度指令 180deg DXL_SetGoalAngle (dev, 1, 180.0); // 1000ms待機 Sleep (1000); // ID:1の制御をOFF DXL_SetTorqueEnable (dev, 1, false); DX2_ClosePort (dev); }
複数IDの制御を個別に開始ないし停止させる。
ライブラリ内のリストに登録されていないIDは無視される。
なお、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetTorqueEnables (TDeviceID dvid, const uint8_t *ids, const bool *ens, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順にtrueで開始、falseで停止を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
複数IDの制御を一括で開始ないし停止させる。
ライブラリ内のリストに登録されていないIDは無視される。
なお、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetTorqueEnablesEquival (TDeviceID dvid, const uint8_t *ids, int num, bool en);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
対象とするID数
trueで開始、falseで停止。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); DXL_GetModelInfo (dev, 1); DXL_GetModelInfo (dev, 2); DXL_GetModelInfo (dev, 3); uint8_t ids[3] = {1, 2, 3}; // ID:1~3の制御をON DXL_SetTorqueEnablesEquival (dev, ids, sizeof (ids), true); DX2_ClosePort (dev); }
対象IDのトルクイネーブル状態を取得する。
bool DXL_GetTorqueEnable (TDeviceID dvid, uint8_t id, bool *en);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
取得したトルクイネーブルの保存先。
処理が正常終了したらtrue、それ以外はfalseを返す。
複数IDのトルクイネーブル状態を取得する。
ライブラリ内のリストに登録されていないIDは無視される。
bool DXL_GetTorqueEnables (TDeviceID dvid, const uint8_t *ids, bool *en, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に取得したトルクイネーブルを保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
DynamixelのコントロールテーブルではPositionと称しているユニークな単位系を持つアイテムは、モデルによって動作角度や分解能が大きく異なります。異なるモデルを混在して運用する場合は非常に不便なため、物理値の角度[deg]を用いてアクセスします。
指定IDへ目標角度を指令する。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAngle (TDeviceID dvid, uint8_t id, double angle);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
目標角度[deg]。中立位置を0[deg]とする。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); DXL_GetModelInfo (dev, 1); DXL_SetOperatingMode (dev, 1, 3); DXL_SetTorqueEnable (dev, 1, true); // ID:1へ角度指令 180deg DXL_SetGoalAngle (dev, 1, 180.0); Sleep (1000); // ID:1へ角度指令 -180deg DXL_SetGoalAngle (dev, 1, -180.0); Sleep (1000); // ID:1へ角度指令 0deg DXL_SetGoalAngle (dev, 1, 0.0); Sleep (1000); DXL_SetTorqueEnable (dev, 1, false); DX2_ClosePort (dev); }
複数IDへ個別の目標角度を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAngles (TDeviceID dvid, const uint8_t *ids, const double *angles, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に目標角度[deg]を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); DXL_GetModelInfo (dev, 1); DXL_GetModelInfo (dev, 2); DXL_GetModelInfo (dev, 3); uint8_t ids[3] = {1, 2, 3}; DXL_SetOperatingModesEquival (dev, ids, sizeof (ids), 3); DXL_SetTorqueEnablesEquival (dev, ids, sizeof (ids), true); // 3軸分の角度を保存したの配列 double angles[3] = { 0.0, 150.0, 300.0 }; // 3軸分の角度指令 DXL_SetGoalAngles (dev, ids, angles, sizeof (ids)); Sleep (2000); DXL_SetTorqueEnablesEquival (dev, ids, sizeof (ids), false); DX2_ClosePort (dev); }
対象IDの現在角度を取得する。
bool DXL_GetPresentAngle (TDeviceID dvid, uint8_t id, double *angle);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
取得した現在角度[deg]の保存先。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <stdio.h> #include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); uint8_t id = 1; DXL_GetModelInfo (dev, id); DXL_SetOperatingMode (dev, id, 3); DXL_SetTorqueEnable (dev, id, true); DXL_SetGoalAngle (dev, id, -180.0); Sleep (1000); DXL_SetGoalAngle (dev, id, 180.0); for (int i = 0; i < 1000; i++) { double pang; // 現在角度取得 if (DXL_GetPresentAngle (dev, id, &pang)) printf ("%5.1r", pang); Sleep (10); } DXL_SetTorqueEnable (dev, id, false); DX2_ClosePort (dev); }
複数IDの現在角度を取得する。
ライブラリ内のリストに登録されていないIDは無視される。
bool DXL_GetPresentAngles (TDeviceID dvid, const uint8_t *ids, double *angles, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に取得した現在角度[deg]を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
制御をOFFにする事で制御が停止するのと合わせて脱力状態になります。それでは現在の角度を保持しつつ止めることができないため、指令されたタイミングの角度を取得してその値をそのまま角度指令し直す機能を設けています。
指定IDを現在角度で停止させる。
動作モードがPosition Controlである事。
bool DXL_StandStillAngle (TDeviceID dvid, uint8_t id);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
処理が正常終了したらtrue、それ以外はfalseを返す。
複数IDを現在角度で停止させる。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事。
bool DXL_StandStillAngles (TDeviceID dvid, const uint8_t *ids, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
DynamixelのコントロールテーブルではVelocityと称しているユニークな単位系を持つアイテムは、モデルによってレンジや分解能が異なります。異なるモデルを混在して運用する場合は非常に不便なため、物理値の角速度[deg/sec]を用いてアクセスします。
指定IDへ目標角速度を指令する。
動作モードがVelocity Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalVelocity (TDeviceID dvid, uint8_t id, double velocity);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
目標角速度[deg/s]。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); DXL_GetModelInfo (dev, 1); DXL_SetOperatingMode (dev, 1, 1); DXL_SetTorqueEnable (dev, 1, true); // 角速度指令 30deg/s DXL_SetGoalVelocity (dev, 1, 30.0); Sleep (1000); // 角速度指令 40deg/s DXL_SetGoalVelocity (dev, 1, 40.0); Sleep (1000); DXL_SetTorqueEnable (dev, 1, false); DX2_ClosePort (dev); }
複数IDへ個別の目標角速度を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがVelocity Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalVelocities (TDeviceID dvid, const uint8_t *ids, const double *velocities, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に目標角速度[deg/s]を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
対象IDの現在角速度を取得する。
bool DXL_GetPresentVelocity (TDeviceID dvid, uint8_t id, double *velocity);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
取得した現在角速度[deg/s]の保存先。
処理が正常終了したらtrue、それ以外はfalseを返す。
複数IDの現在角速度を取得する。
ライブラリ内のリストに登録されていないIDは無視される。
bool DXL_GetPresentVelocities (TDeviceID dvid, const uint8_t *ids, double *velocities, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に取得した現在角速度[deg/s]を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
角度と合わせて角速度を同時に指令することで、指定角度へゆっくり動かすと行った動作を行わせる事ができます。
なお、Dynamixelの角速度制御の分解能はさほど大きくないため、角速度が低い場合は誤差が大きくなります。
指定IDへ目標角度と目標角速度を指令する。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAngleAndVelocity (TDeviceID dvid, uint8_t id, double angle, double velocity);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
目標角度[deg]。
目標角速度[deg/s]。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); uint8_t id = 3; DXL_GetModelInfo (dev, id); DXL_SetOperatingMode (dev, id, 3); DXL_SetTorqueEnable (dev, id, true); // 現在角度から-170degの角度へ30deg/sの角速度で DXL_SetGoalAngleAndVelocity (dev, id, -170.0, 30.0); Sleep (10000); // 現在角度から160degの角度へ40deg/sの角速度で DXL_SetGoalAngleAndVelocity (dev, id, 160.0, 40.0); Sleep (10000); DXL_SetTorqueEnable (dev, id, false); DX2_ClosePort (dev); }
複数IDへ個別の目標角度と目標角速度を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAnglesAndVelocities (TDeviceID dvid, const uint8_t *ids, const PAngleVelocity anglevelocity, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に目標角度[deg]と目標角速度[deg/s]を保持する構造体の配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
角度と合わせて時間を同時に指令します。ライブラリ内では時間を角速度(=(目標角度-現在角度)/移動時間)に変換して指令します。
なお、Dynamixelの角速度制御の分解能はさほど大きくないため、移動速度が長い場合は誤差が大きくなります。
また事前にDXL_SetDriveModeもしくはDXL_SetDriveModesEquivalでVelocity-Base Profile(出荷時デフォルト)を指定しておく必要があります。
最新のファームウェアを備えたDynamixelの場合は角度と時間2のAPIを利用することを推奨します。
指定IDへ目標角度と現在角度から目標角度までの移動時間を指令する。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAngleAndTime (TDeviceID dvid, uint8_t id, double angle, double sec);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
目標角度[deg]。
移動時間[sec]。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); uint8_t id = 3; DXL_GetModelInfo (dev, id); DXL_SetOperatingMode (dev, id, 3); DXL_SetTorqueEnable (dev, id, true); // 現在角度から-150degの角度へ3秒で移動 DXL_SetGoalAngleAndTime (dev, id, -150.0, 3.0); Sleep (4000); // 現在角度から180degの角度へ5秒で移動 DXL_SetGoalAngleAndTime (dev, id, 180.0, 5.0); Sleep (6000); DXL_SetTorqueEnable (dev, id, false); DX2_ClosePort (dev); }
複数IDへ個別の目標角度と現在角度から目標角度までの移動時間を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAnglesAndTime (TDeviceID dvid, const uint8_t *ids, const double *angles, int num, double sec);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に目標角度[deg]を保持する配列の保存先。
対象とするID数
目標移動時間[sec]。
処理が正常終了したらtrue、それ以外はfalseを返す。
角度と合わせて時間を同時に指令します。目的は先のAPIと同様ですが、Dynamixelの新しいファームウェアに備わっているTime-Base Profileを用いて指令するため、指定時間通りに目標角度に到達します。
事前にDXL_SetDriveModeもしくはDXL_SetDriveModesEquivalでTime-Base Profileを設定しておく必要があります。
指定IDへ目標角度と現在角度から目標角度までの移動時間を指令する。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAngleAndTime2 (TDeviceID dvid, uint8_t id, double angle, double sec);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
目標角度[deg]。
移動時間[sec] (0.0~32.767)。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); uint8_t id = 3; DXL_GetModelInfo (dev, id); // Time-Base profileを設定 DXL_SetDriveMode (dev, id, 0x04); DXL_SetOperatingMode (dev, id, 3); DXL_SetTorqueEnable (dev, id, true); // 現在角度から-150degの角度へ3秒で移動 DXL_SetGoalAngleAndTime2 (dev, id, -150.0, 3.0); Sleep (4000); // 現在角度から180degの角度へ5秒で移動 DXL_SetGoalAngleAndTime2 (dev, id, 180.0, 5.0); Sleep (6000); DXL_SetTorqueEnable (dev, id, false); DX2_ClosePort (dev); }
複数IDへ個別の目標角度と現在角度から目標角度までの移動時間を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPosition Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalAnglesAndTime2 (TDeviceID dvid, const uint8_t *ids, const double *angles, int num, double sec);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に目標角度[deg]を保持する配列の保存先。
対象とするID数
移動時間[sec] (0.0~32.767)。
処理が正常終了したらtrue、それ以外はfalseを返す。
指定IDへ目標電流を指令する。
プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalCurrent (TDeviceID dvid, uint8_t id, double current);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
目標電流[mA]。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); DXL_GetModelInfo (dev, 1); DXL_SetOperatingMode (dev, 1, 0); DXL_SetTorqueEnable (dev, 1, true); // 電流指令 200mA DXL_SetGoalCurrent (dev, 1, 200.0); Sleep (1000); // 電流指令 -200mA DXL_SetGoalAngle (dev, 1, -200.0); Sleep (1000); DXL_SetTorqueEnable (dev, 1, false); DX2_ClosePort (dev); }
複数IDへ個別の目標電流を指令する。
ライブラリ内のリストに登録されていないIDは無視される。
プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalCurrents (TDeviceID dvid, const uint8_t *ids, const double *currents, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に目標電流[mA]を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
指定IDの現在電流を取得する。
bool DXL_GetPresentCurrent (TDeviceID dvid, uint8_t id, double *current);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
取得した現在電流[mA]の保存先。
処理が正常終了したらtrue、それ以外はfalseを返す。
複数IDの現在電流を取得する。
ライブラリ内のリストに登録されていないIDは無視される。
bool DXL_GetPresentCurrents (TDeviceID dvid, const uint8_t *ids, double *currents, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に取得した現在電流[mA]を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
指定IDへ目標PWMを指令する。
動作モードがPWM Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalPWM (TDeviceID dvid, uint8_t id, double pwm);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
目標PWM[%]。
処理が正常終了したらtrue、それ以外はfalseを返す。
#include <dx2lib.h> void main (void) { TDeviceID dev = DX2_OpenPort ("\\.\COM10", 57600); DXL_GetModelInfo (dev, 1); DXL_SetOperatingMode (dev, 1, 16); DXL_SetTorqueEnable (dev, 1, true); // PWM指令 50% DXL_SetGoalPWM (dev, 1, 50.0); Sleep (1000); // PWM指令 -40% DXL_SetGoalPWM (dev, 1, -40.0); Sleep (1000); DXL_SetTorqueEnable (dev, 1, false); DX2_ClosePort (dev); }
複数IDへ個別の目標PWMを指令する。
ライブラリ内のリストに登録されていないIDは無視される。
動作モードがPWM Controlである事と、プロトコルV2のDynamixelは明示的に制御を開始しない限りモータの制御を行わない。
bool DXL_SetGoalPWMs (TDeviceID dvid, const uint8_t *ids, const double *pwms, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に目標PWM[%]を保持する配列の保存先。
対象とするID数
処理が正常終了したらtrue、それ以外はfalseを返す。
指定IDの現在PWMを取得する。
bool DXL_GetPresentPWM (TDeviceID dvid, uint8_t id, double *pwm);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID (0~252)。
取得した現在PWM[%]の保存先。
処理が正常終了したらtrue、それ以外はfalseを返す。
複数IDの現在PWMを取得する。
ライブラリ内のリストに登録されていないIDは無視される。
bool DXL_GetPresentPWMs (TDeviceID dvid, const uint8_t *ids, double *pwms, int num);
DX?_OpenPortで開いた際のTDeviceID。
対象とするID一覧を保持する配列の保存先。
idsで指定されたID順に取得した現在PWM[%]を保持する配列の保存先。
対象とするID数
モーションは時間を追って変化する目標値を任意の時間で区切って羅列したデータを予め作っておき、そのデータをサーボの補間機能を活用しながら再生するもの。ここでは2個のサーボを角度制御と速度制御によるモーションを使って運転してみる。
mt.start(0, IDs, (jp200.Pose(2, [ 400.0, 800.0 ], 1, 500, 'TP=1000'),)) mt.start(0, IDs, (jp200.Pose(2, [ -800.0, -400.0 ], 1, 500, 'TP=1000'),)) mt.start(0, IDs, (jp200.Pose(2, [ 0.0, 0.0 ], 1, 500, 'TP=1000'),))
分離しただけでは意味が無いので、数値変換してID毎にリストに保存してみる。RRコマンドは';'をデリミタとしてデータが連なって返ってくるのでsplitを使って更に分割。
¶<pre class="brush: python;">
¶モーションは時間を追って変化する目標値を羅列したデータを予め作っておき、そのデータを再生するもの。ここでは2個のサーボを角度制御と速度制御によるモーションを使って運転してみる。
¶モーションは時間を追って変化する目標値を羅列したデータを予め作っておき、そのデータを再生するもの。制御モードに応じて目標値は異なる。
ここでは2個のサーボを角度制御と速度制御のモーションで運転してみる。
import serial import jp200class as jp200 portname = '/dev/ttySC2' #portname = '\\.\COM12' baudrate = 1000000 HomeOffsetAngle = {1:180.0, 2:180.0} # Target ID IDs = (1,2) # Motion data m0 = ( jp200.Pose(1, [ 0.0, 0.0 ], 1, 1000, 'TP=1000'), jp200.Pose(1, [ 90.0, 10.0 ], 1, 1500, ''), jp200.Pose(1, [ 180.0, 20.0 ], 1, 1500, ''), jp200.Pose(1, [ -90.0, -30.0 ], 1, 1500, ''), ) m1 = ( jp200.Pose(1, [ 0.0, 0.0 ], 1, 1000, 'TP=1000'), jp200.Pose(1, [ 200.0, 200.0 ], 1, 500, ''), jp200.Pose(1, [ 100.0, 100.0 ], 1, 500, ''), jp200.Pose(1, [-100.0,-100.0 ], 1, 500, ''), jp200.Pose(1, [ 0.0, 0.0 ], 1, 500, ''), ) ser = serial.Serial(portname, baudrate) pk = jp200.packet(ser, 0.02) mt = jp200.motion(pk, HomeOffsetAngle) s = '' while s != 'q': s = input('>') if s == '1': mt.start(0, IDs, m0) if s == '2': mt.start(0, IDs, m1) if s == '3': mt.start(0, IDs, (jp200.Pose(2, [ 400.0, 800.0 ], 1, 500, 'TP=1000'),)) if s == '4': mt.start(0, IDs, (jp200.Pose(2, [ -800.0, -400.0 ], 1, 500, 'TP=1000'),)) if s == '5': mt.start(0, IDs, (jp200.Pose(2, [ 0.0, 0.0 ], 1, 500, 'TP=1000'),)) if s == 'e': print('stop') for i in range(4):mt.stop(i) for i in range(4):mt.stop(i) del mt, pk ser.close()
ルネサスエレクトロニクス社(以後renesas)のコンポーネントを使用したブラシ付きDCモータを簡易的に制御するPoC(概念実証)ベンチ。
外観は既成のサーボモータと酷似し、簡単なテキストコマンドにより角度・速度・電流の各制御指令を行う。
型番 | JP200 | |
PCBリビジョン | E192(4層) | |
動作温度範囲 | -10~50℃ 結露なきこと | |
外形寸法 | 41×21×30.5mm(突起部除く) | |
重量 | 41.5g(アクセサリを除く) | |
電源 | DC6~12V(絶対最大定格DC28V) | |
モータ | コアドブラシ付DCモータ | |
ストールトルク | 1.37N•m(at 11.1V, 1.3A) | |
無負荷回転数 | 50rpm(at 11.1V) | |
減速比 | 1/312.4 | |
最大動作角度 | 位置決め制御時:0~360°(16ビット分解能),Endless Turn | |
コネクタ等 | JST B3B-ZR x2 LED x1 | |
I/F仕様 | 半二重TTLレベル |
機構はKONDO KRS-2500シリーズと互換性があるので、それ用に提供されるパーツが流用可能。
Pats Name | JST Parts Number |
基板用ヘッダー | B3B-ZR |
ハウジング | ZHR-3 |
ターミナル | SZH-002T-P0.5 |
端子番号 | 信号名 | |
1 | Signal | シングルエンドのシリアル通信信号 双方向 |
2 | VDD | 電源のプラス側 |
3 | GND | 電源のマイナス側 シリアル通信信号の基準電位と共有 |
初期ブートローダ
https://www.besttechnology.co.jp/download/JP200_bootloader_20230429.7z
- fsp4.4
2023/9/23更新
https://www.besttechnology.co.jp/download/JP200_bootloader_20230923.7z
- fsp4.6
- ボーレートを3M[bps]から1M[bps]へ変更
- 一部のホスト側のI/Fにおいてバスの切り替えタイミングがブートローダの送信とコンフリクトし文字化けする事があったため、送信前に100usの遅延を挿入
本ブートローダはシングルチップモードで動作するアプリケーションプログラムであり、MPUが持つブートプログラムとは異なる。
主たる目的はJP200が備えている半二重シリアルI/Fを介して任意のプログラムを転送し内蔵フラッシュメモリに書き込む事と、そのプログラムの実行手段を提供する事にある。
本ブートローダから実行できるプログラムにはいくつか制約がある。MPUが想定しているスタートアドレスが0x0番地なのに対し、0x0番地は本ブートローダ本体が書き込まれているため、0x00010000をスタートアドレスに設定する必要がある。またオフセットしたアドレスから起動している前提で各レジスタが初期化されている必要がある。
次に本ブートローダのダウンロードがサポートするファイルは、メモリ上に展開した状態をそのままバイナリデータ化したもののみ。HEX・MOT・SREC・ELFといったファイルフォーマットには対応していない。
e2studioでプロジェクトを作成する際の要点はこちらを参考に。
コールドスタートやホットスタート、ユーザープログラムの状態による分岐を以下に記載する。
電源を印加するとブートローダはそのユーザプログラムにジャンプ。
I/Fから'!'を連続で受信させながら電源を印加するとブートローダはコマンドモードで待機。
電源を印加するとブートローダはコマンドモードで待機。
ソフトウェアリセット後にブートローダはコマンドモードで待機。
ユーザプログラムの転送にはレガシーなXMODEM/1kプロトコルのみに対応1し、I/Fの設定は1Mbps 8N1を要求する。XMODEMが半二重を前提としているので、ホスト側のターミナルソフトウェアはI/Fが半二重であることを特段意識する事はない。
ユーザープログラムの転送手順は以下の通り。
まず'e'を送信してフラッシュメモリの消去を起動する。
RA4M2 BL1.2>e(y/n)
ここで'y'を送信すると実際に消去される。
RA4M2 BL1.2>e(y/n)y OK RA4M2 BL1.2>
次に'w'を送信して転送モードを起動する。
RA4M2 BL1.2>w(y/n)
ここでyを入力したら、すかさず転送プロトコルに従って任意のバイナリファイルの送信を開始させる。成功すれば<OK>、失敗すると<NG>と表示される。
RA4M2 BL1.2>w(y/n)y OK RA4M2 BL1.2>
'g'を送信してフラッシュメモリに書き込まれたユーザプログラムを実行する。
RA4M2 BL1.2>g
これらの一連の操作をTeraTermで行う際のマクロを紹介しておく。
初期ファームウェア
https://www.besttechnology.co.jp/download/JP200_20230721.7z
- fsp4.5
2023/9/18更新
https://www.besttechnology.co.jp/download/JP200_20230918.7z
- fsp4.6
- 符号付32ビットデータに-2147483648が設定されている場合、それを読み出すとパケットデータが異常になるのを修正
- 各ゲインの最大値を10000に変更
- パワーサイクル時の不揮発データの適用を活性化
- 単位の無かった速度を回転数[x1000 rot/sec]に変更
- 偏差によるPWM制限機能としてOffsetAngle, AngleFlexibility, CurrentFlexibilityを追加
- 指定時間で最終指令値から目標値へ遷移させる機能としてTransitionMode, TransitionTimeを追加
それに伴いTA,TV,TCコマンドにTransitionModeとTransitionTimeの指定を追加- 角度制御時の動作可能範囲としてAngleLimitを追加
- I2Cの処理が破綻するために最適化を抑止していたが、プロジェクトの最適化レベルを-O3にし、最適化による影響を各ソースでコントロール
- 文字列処理の大半を標準ライブラリに置き換え
- パケット処理を見直し応答時間を改善
- 多少オーバフロー意識したコードに修正
それに伴い各所のバッファをstatic化しアロケート時のオーバヘッドを軽減- エンコーダエラー時に制御を停止させ、LEDを点滅させる
リカバリはパワーサイクルのみ
2023/9/26更新
https://www.besttechnology.co.jp/download/JP200_20230926.7z
- fsp4.6
- デフォルトボーレートを1M[bps]に変更
- 応答遅延としてReturnDelayTimeを追加
- 監視機能としてMonInfo[0]~MonInfo[4]を追加
- 監視機能の追加に伴いControlDurationTime, NoCommTime, AngleViation, VelocityViation, CurrentViationを追加
- コントロールテーブルのサイズが更新されたため、本ファームウェアを適用すると不揮発領域は初期化される
2023/10/11更新
https://www.besttechnology.co.jp/download/JP200_20231011.7z
- fsp4.6
- 各制御にフィードフォワードを追加
それに伴いSG0~2コマンドの引数を追加- 各制御ゲインの上限値を30000に変更
- 各制御における指令値に加算するオフセット指令値を追加
- 日替わりで更新している事もあり、ひとまずVERコマンドを追加
- TA/TV/TC/SGコマンドの数値切り出しに失敗するようなので処理方法を変更
- RRコマンドで温度の更新がなされないのを修正
- 通信が復帰しないケースが散見されたので、暫定的にuartのコールバック中にエラー処理を追加
- コントロールテーブルのサイズ及びアイテムのインデックスが更新されたため、本ファームウェアを適用すると不揮発領域は初期化される
2023/10/30更新
https://www.besttechnology.co.jp/download/JP200_20231030.7z
- fsp5.0
- すっかり失念していたCRCの付与に対応
- 非接触エンコーダとの通信に使っているCRCルーチンを分離
- 少し攻め気味だった速度制御のゲインを低めに変更
- 制御モードに静止を追加
- 大きめのTransitionTimeと目標値を指定すると容易にオーバーフローするので計算は64bitに変更
- 角度制御以外から角度制御に切り替えた直後に遷移時間を指定して運転すると開始角度が初期化されないのを修正
- 同梱のjp200_test.exeを更新
一般的な算数通りに演算してモータをフィードバック制御しているのみで、特筆すべき所は無い。唯一あるとすればシリアルサーボの類にありがちなバイナリベースの通信プロトコルを一切使わず、全て目視で判断できるテキストのみの電文で処理している点。なおI/Fは1ワイヤの半二重通信を前提とし、昨今のデバイスの能力に任せてメガbpsでの通信を基本とする。
ファームウェアはe2studioを用いて作られており、renesasから提供されるfspを一通り使用した。開発当初はSWD端子にSEGGER社のJ-Linkを接続してデバッグを行っていたが、アプリケーションがテキストによる通信を行うもののため、最終的にはブートローダを介したファームウェアの書き換えとprintfデバッグを併用した。
なおソース単位で機能が囲われているかのように見えるかも知れないが、シングルソースだったものを後にそれっぽく分割した程度である。またC++でクラス化していた時分もあったが、積極的に使い始めた途端に破綻したのでCに戻した経緯がある。
ホストもしくはサーボのパケットである事を区別するため、それぞれ不等号の「<>」と括弧の「()」のヘッダとフッダで囲われた範囲を有効なパケットとする。このヘッダとフッダに挟まれた中に必要な命令や応答を記述するが、使用できるのはアスキー文字の「0」~「9」, 「A」~「Z」, 「a」~「z」,「#」,「=」, 「+」, 「-」, 「;」, 「:」, 「,」のみで、それ以外の文字コードが括弧内に含まれているとパケットごと破棄する。
命令や応答の詳細は後述するとして、IDが各々1と2に設定された2つのサーボにホストからパケットを送信し、2つのサーボから応答が返った場合の例を示す。
ホスト:<#1EX=1TA=-300#2EX=2TV=300> サーボ:(#1EX=OKTA=OK)(#2EX=OKTV=OK)
全てが想定通りな場合、2つのサーボはホストのパケットに記述されたID順にパケットを返信する。またホストのパケットに存在しないIDが含まれている場合は、それ以後のIDを持つサーボからの応答は返らない。
なおホストのパケットを受け取ったサーボは必ずパケットを返信するため、ホストはその返信されたパケットを受信しなくてはならない。返信のパケットの受信を待たずしてホストが新たなパケットを送信すると、ホストとサーボのパケット同士がコンフリクトし半二重通信が破綻する。また何らかの要因でサーボから応答のパケットを受信できなかった場合に備え、タイムアウト等で受信を諦める処理が必要である。
複数軸の応答となると、受信を終えるまでの時間はその軸数分かかり、条件によってはホストの指令周期を担保できなくなる場合がある。応答を必要としないのであれば、ホストのパケットを括る括弧を「[]」に変更して送信する事で、サーボは応答を返さなくなる。
更にホストのパケットを括る括弧を「{}」に変更した場合は、末尾の括弧の直前に「:」と10進数でCRC8を付与する必要がある。そのパケットを正常に受信したサーボの応答も同様に「:」とCRC8が付与される。ここで使用したCRC8の詳細はプロジェクトのソースとPythonの送受信クラスを参考にしてもらうとして、これによりテキストかつシングルエンドによる脆弱な通信環境が僅かながら堅牢になる。
ホスト:{#0EX=1TA=9000TP=1000#1EX=1TA=9000TP=1000:170} サーボ:(#0EX=OKTA=OKTP=OK:70)(#1EX=OKTA=OKTP=OK:195)
以下の「コマンド」に記述された「<」「>」「[」「]」はコマンドの構文の説明としての記号であり、「<>」内は必須、「[]」内はオプションを説明しているもので、実際にパケット中にそれらの記号を記述してはならない。また数値は全て10進数表記である。
機能 | コマンド | 指令 |
応答 | ||
ID指定 | #<n> | n=0~254 コマンドの送信先を指定するために必ず記述必要があり、複数のIDを混在させる事も可 |
n=0~254 同一IDを複数指定した場合、1つにまとめられる | ||
制御モード変更 | EX=<n> | n=0~8,15 0:制御停止, 1:角度制御, 2:角速度制御, 3:角度+速度制御, 4:電流制御, 5:角度+電流制御, 6:速度+電流制御, 7:角度+速度+電流制御, 8:PWM, 14:現在位置を維持したまま静止, 15:回生ブレーキ |
n=OK または NG | ||
角度指令 | TA=<n[[;m[;t]]]> | n=指令値[x100 deg] m=遷移軌跡 t=遷移時間[ms] |
n=OK または NG | ||
角速度指令 | TV=<n[[;m[;t]]]> | n=指令値[x1000 rot/sec] m=遷移軌跡 t=遷移時間[ms] |
n=OK または NG | ||
電流指令 | TC=<n[[;m[;t]]]> | n=指令値[mA] m=遷移軌跡 t=遷移時間[ms] |
n=OK または NG | ||
PWM指令 | TP=<n> | n=指令値[‰] |
n=OK または NG | ||
現在角度取得 | CA | 現在角度を取得 |
CA=<n> | n=フィードバック値[x100 deg] | |
現在角速度取得 | CV | 現在角速度を取得 |
CV=<n> | n=フィードバック値[x1000 rot/sec] | |
現在電流取得 | CC | 現在角電流を取得 |
CC=<n> | n=フィードバック値[mA] | |
現在PWM取得 | CP | 現在PWMを取得 |
CP=<n> | n=フィードバック値[‰] | |
MPU温度取得 | CT0 | MPU温度を取得 |
CT0=<n> | n=フィードバック値[x10 degC] | |
アンプ温度取得 | CT1 | アンプ温度を取得 |
CT1=<n> | n=フィードバック値[x10 degC] | |
モータ温度取得 | CT2 | モータ温度を取得 |
CT2=<n> | n=フィードバック値[x10 degC] | |
電源電圧取得 | CB | 電源電圧を取得 |
CB=<n> | n=フィードバック値[mV] | |
ステータス取得 | ST | 内部ステータスを取得 |
ST=<n> | n=ステータス | |
位置制御ゲイン設定 | SG0=<p[;i[;d[;f]]]> | p=比例ゲイン, i=積分ゲイン, d=微分ゲイン, f=フィードフォワードゲイン ゲインはいずれも整数で、1/1000の固定小数点として扱われる |
SG0=<n> | n=OK または NG | |
角速度制御ゲイン設定 | SG1=<p[;i[;d[;f]]]> | p=比例ゲイン, i=積分ゲイン, d=微分ゲイン, f=フィードフォワードゲイン ゲインはいずれも整数で、1/1000の固定小数点として扱われる |
SG1=<n> | n=OK または NG | |
電流制御ゲイン設定 | SG2=<p[;i[;d[;f]]]> | p=比例ゲイン, i=積分ゲイン, d=微分ゲイン, f=フィードフォワードゲイン ゲインはいずれも整数で、1/1000の固定小数点として扱われる |
SG2=<n> | n=OK または NG | |
テーブル書込 | WR<ind>=<d1>[;<d2>][;<d3>]...[;<dN>] | ind=開始インデックス d1~dN=データ列 |
WR<ind>=<n> | ind=開始インデックス n=OK または NG | |
テーブル読出 | RR<ind>;<n> | ind=開始インデックス n=データ個数 |
RR<ind>=<d1>[;<d2>][;<d3>]...[;<dN>] | ind=開始インデックス n=データ列 | |
不揮発化 | WD | NVMとあるアイテムの情報をデータフラッシュに保存する |
WD=<n> | n=OK または NG | |
初期化 | INI | 全ての情報をデフォルトに戻しデータフラッシュに保存する |
INI=<n> | n=OK または NG | |
再起動 | RB | ソフトリセットを行いブートローダをコマンドモードで立ち上げる |
RB=<n> | n=OK または NG | |
構築日取得 | VER | プロジェクトの再構築日時を取得 |
VER=<Y;M;D;h;m;s> | Y/M/D:日付、h/m/s:時刻 |
TAやTVといったコマンドは、実際には次に示すコントロールテーブルへのアクセスに変換される。コントロールテーブルへの直接的なアクセスにはWRやRRコマンドを使用する。
Index | Item | Access | Default Value | Type/Range | |
0 | ID | R/W (NVM) | 1 | 0 ~ 254 | |
1 | Baudrate | R/W (NVM) | 10 | 0 ~ 14 | |
2 | OffsetAngle | R/W (NVM) | 0 | -18000 ~ 18000 | |
3 | LED | R/W | 0 | 0 ~ 1 | |
2 | ReturnDelayTime | R/W (NVM) | 0 | 0 ~ 1000 | |
5 | SystemStatus | R | - | - | |
6 | AngleFlexibility | n_compliance | R/W (NVM) | 0 | -36000 ~ 0 |
7 | n_deadzone | R/W (NVM) | 0 | -36000 ~ 0 | |
8 | p_deadzone | R/W (NVM) | 0 | 0 ~ 36000 | |
9 | p_compliance | R/W (NVM) | 0 | 0 ~ 36000 | |
10 | CurrentFlexibility | n_compliance | R/W (NVM) | 0 | -1300 ~ 0 |
11 | n_deadzone | R/W (NVM) | 0 | -1300 ~ 0 | |
12 | p_deadzone | R/W (NVM) | 0 | 0 ~ 1300 | |
13 | p_compliance | R/W (NVM) | 0 | 0 ~ 1300 | |
14 ~ 17 | (reserve) | R | - | ||
18 | AngleLimit | min | R/W (NVM) | -2147483648 | -2147483648 ~ 2147483647 |
19 | max | R/W (NVM) | 2147483647 | -2147483648 ~ 2147483647 | |
20 | AngleCtrlGain | Kp | R/W (NVM) | 2000 | 0 ~ 30000 |
21 | Ki | R/W (NVM) | 5000 | 0 ~ 30000 | |
22 | Kd | R/W (NVM) | 0 | 0 ~ 30000 | |
23 | Kf | R/W (NVM) | 0 | 0 ~ 30000 | |
24 | delta_t | R/W (NVM) | 1 | 1 ~ 1000 | |
25 | VelocityCtrlGain | Kp | R/W (NVM) | 400 | 0 ~ 30000 |
26 | Ki | R/W (NVM) | 600 | 0 ~ 30000 | |
27 | Kd | R/W (NVM) | 0 | 0 ~ 30000 | |
28 | Kf | R/W (NVM) | 0 | 0 ~ 30000 | |
29 | delta_t | R/W (NVM) | 32 | 1 ~ 1000 | |
30 | CurrentCtrlGain | Kp | R/W (NVM) | 900 | 0 ~ 30000 |
31 | Ki | R/W (NVM) | 1500 | 0 ~ 30000 | |
32 | Kd | R/W (NVM) | 0 | 0 ~ 30000 | |
33 | Kf | R/W (NVM) | 0 | 0 ~ 30000 | |
34 | delta_t | R/W (NVM) | 2 | 1 ~ 1000 | |
35 | TransitionMode | R/W | 0 | 0 ~ 3 | |
36 | TransitionTime | R/W | 0 | 0 ~ 300000 | |
37 | Excitation | R/W | 0 | 0 ~ 15 | |
38 | TargetAngle | R/W | - | -2147483648 ~ 2147483647 | |
39 | TargetVelocity | R/W | 0 | -2000 ~ 2000 | |
40 | TargetCurrent | R/W | 0 | -1300 ~ 1300 | |
41 | TargetPWM | R/W | 0 | -1000 ~ 1000 | |
42 | OffsetTargetAngle | R/W | 0 | -2147483648 ~ 2147483647 | |
43 | OffsetTargetVelocity | R/W | 0 | -2000 ~ 2000 | |
44 | OffsetTargetCurrent | R/W | 0 | -1300 ~ 1300 | |
45 | CurrentAngle | R | - | - | |
46 | CurrentVelocity | R | - | - | |
47 | CurrentCurrent | R | - | - | |
48 | CurrentPWM | R | - | - | |
49 | CurrentRawPosition | R | - | 0~65535 | |
50 | CurrentBusVoltage | R | - | - | |
51 | CurrentMPUTemp | R | - | - | |
52 | CurrentAMPTemp | R | - | - | |
53 | CurrentMotorTemp | R | - | - | |
54 | AngleViation | R | - | - | |
55 | VelocityViation | R | - | - | |
56 | CurrentViation | R | - | - | |
57 | SysTickTime | R | - | - | |
58 | ControlDurationTime | R | - | - | |
59 | NoCommTime | R | - | - | |
60 ~ 96 | AngleCorrection[0~36] | R/W (NVM) | 0 ~ 36000 | ||
97 | MonInfo[0] | ItemIndex | R/W (NVM) | -1 | -1 ~ 131 |
98 | min | R/W (NVM) | 0 | -2147483648 ~ 2147483647 | |
99 | max | R/W (NVM) | 0 | -2147483648 ~ 2147483647 | |
100 | time | R/W (NVM) | 0 | 0 ~ 4294967295 | |
101 | DetectMode | R/W (NVM) | 0 | 0 ~ 255 | |
102 | ProcMode | R/W (NVM) | 0 | 0 ~ 255 | |
103 | DurationTime | R | - | 0 ~ 4294967295 | |
104 ~ 110 | MonInfo[1] | ||||
111 ~ 117 | MonInfo[2] | ||||
118 ~ 124 | MonInfo[3] | ||||
125 ~ 131 | MonInfo[4] |
通信速度[bps]のインデックス。
Value | Baudrate[bps] |
0 | 9600 |
1 | 19200 |
2 | 38400 |
3 | 57600 |
4 | 100000 |
5 | 115200 |
6 | 200000 |
7 | 230400 |
8 | 460800 |
9 | 500000 |
10 | 1000000 |
11 | 2000000 |
12 | 3000000 |
13 | 4000000 |
14 | 6000000 |
システムステータス。
Bit | Name | Description |
31~14 | reserve | 常時0 |
13 | Match monitoring conditions | 監視一致 |
12 | Encoder Error | エンコーダ値取得失敗 |
11~8 | reserve | 常時0 |
7~0 | Encoder Status | エンコーダ値取得時ステータス |
メインループが角度制御の時、指令角度と現在角度の偏差によってPWM出力の制限を行う。
n_compliance[x100 deg]<=n_dedzone[x100 deg]<=p_deadzone[x100 deg]<=p_compliance[x100 deg]とする事。
deadzoneの間は内部の制御量がリセットされる。
メインループが電流制御の時、指令電流と現在電流の偏差によってPWM出力の制限を行う。
n_compliance[mA]<=n_dedzone[mA]<=p_deadzone[mA]<=p_compliance[mA]とする事。
deadzoneの間は制御量がリセットされる。
各制御対象におけるPID制御の各ゲイン(x1000)とフィードフォワードゲイン(x1000)、離散積分時間[ms]。
各ゲインは1/1000した上で演算に使用される。
Item | Details |
Kp | x1000 |
Ki | x1000 |
Kd | x1000 |
Kf | x1000 |
delta_t | [ms] |
delta_t は各制御対象の固定サンプリング時間のため変更は推奨しない。
ブロック図は敢えて示さないが、単純なPID演算とカスケード及びフィードフォワード。演算上のオーバーフローなどは一切考慮していないので、極端な数値指定は禁物。
制御モード。
先頭に記述したものがメジャーループ、それ以降に複数記述されているものがマイナーループのカスケード制御となる。PWMについてはいずれの場合においても最終出力段のリミッタとして機能する。
Value | Control Mode |
0 | 制御停止(フリー) |
1 | Angle (+ PWM) |
2 | Velocity (+ PWM) |
3 | Angle + Velocity (+ PWM) |
4 | Current (+ PWM) |
5 | Angle + Current (+ PWM) |
6 | Velocity + Current (+ PWM) |
7 | Angle + Velocity + Current (+ PWM) |
8 | PWM |
14 | 現在位置で静止(+ PWM) |
15 | 制御停止(Brake) |
目標角度[x100 deg]。
数値を増加させるとサーボの出力軸は時計回りに回転する。
制御時の目標角度はAngleLimitの上下限値に制限される。
動作範囲が限られた機構に適用する場合は、角度センサの中央値である18000(180[deg])を動作範囲の中央位置近傍とする事。
目標PWM[‰]。
数値を増加させるとサーボの出力軸は時計回りに回転する。
なおPWMモード以外の制御モードにおいて本PWM値が最終段のリミッタとして機能するため、適切な値を設定した上で使用する事。
16bitのエンコーダ値を角度に変換するためのルックアップテーブル。
基準となるエンコーダ値は以下の37箇所で、その時の角度[x100 deg]を設定。
0, 1820, 3641, 5461, 7282, 9102,10923,12743,14563,16384,18204,20025,21845,23665,25486,27306,29127,30947,32768,34588,36408,38229,40049,41870,43690,45510,47331,49151,50972,52792,54613,56433,58253,60074,61894,63715,65535
デフォルトは単純な一次関数で算出した角度[x100 deg]を設定。
なお現状ではセンサそのものが未キャリブレーション状態なのと、一定のPWM値で高速回転させるとセンサのキャリブレーションが働いて自動的に校正されてしまうため、本値を編集する事は推奨しない。
異常検出を専用に備えているものではないため、それを補完する目的で装備した監視機能。
5つの監視と指定された条件に一致した時の挙動を設定可能。
ItemIndex | min/maxと比較するアイテムのインデックス |
min | 比較時の最小値 |
max | 比較時の最大値 |
time | DetectModeの一致が継続しProcModeの動作を行うまでの時間 |
DetectMode | 比較条件 0:なし 1:min≤value and value≤max 2:min≥value or value≥max 3:min==value or value==max |
ProcMode | DetectModeの一致が指定時継続した時の処理 0:なし 1:演算出力 75%制限(制御継続) 2:演算出力 50%制限(制御継続) 3:演算出力 25%制限(制御継続) 4:演算出力 10%制限(制御継続) 5:演算出力 0%制限(制御継続) 6:フリー(制御停止) 7:ストール(再運転不可) |
DurationTime | DetectModeが一致している時間(Read Only) |
ProcModeが発動するとLEDは点滅し、DetectModeの条件から外れるとLEDは消灯する(ProcMode=7を除く)。
ProcModeの0~5が発動している状態でDetectModeの条件が外れると、制限のない制御に復帰する。
監視を複数指定した場合、ProcModeの値が大きいものが優先して実行される。
ここではホストとしてPCを使用する場合を紹介する。
PCにはサーボと通信を行うためのI/Fは装備されていないため、USBポートを介したI/Fを増設して使用する。参考に既成のI/Fをこちらに紹介するが、ここではそのうちDynamixel Starter Kit Bに使用されるI/Oボードを使用した。このI/OボードはサーボとのI/Fの他に、供給用のACアダプタ用が接続できるジャックを備えているため、全ての接続がI/Oボードに集中し構成がシンプルになる。なお搭載しているUSBシリアル変換ICの仕様により、最大通信速度が3Mbpsとなる。
ファームウェアの解説にある通りサーボはテキストベースの通信プロトコルを備えているため、汎用のシリアル通信ターミナルが使用できる。ここではTeraTermを使用して操作する事とした。
TeraTermのシリアルポートの設定は次のように。
またPCから送信するパケットを可視化するために、端末のローカルエコーにチェックを入れておく。
では具体的に通信を行いながら試運転してみる。
手始めにLEDを使った動作の確認を行う。なお送信パケットの末尾に改行などターミネータキャラクタを付与してはならない。
<#1WR3=1> LEDの点灯を指令 (#1WR=OK) <#1WR3=0> LEDの消灯を指令 (#1WR=OK)
サーボに備わったLEDがパケットに応じて明滅し、応答のパケットが返信されるのを確認できれば最低限の動作環境が整っている事になる。
次にモータを動かす。
内部ではパケットで指定されたコマンドを処理がなされた上で動作するため、必要なコマンドを全て羅列した1つのパケットで処理を完結させる事ができる。
また各制御の最終段にPWM制御(リミッタ)が備わっており、そのリミッタを解除するためにもTPを併記する必要がある。更にカスケード制御時のマイナーコントローラへの指令値は、いずれもその絶対値を使用する。
例えばID=1のサーボを角度を-90deg、回転数を100[r/s]、電流を60mA、PWMリミットなしで制御する場合は以下のように記述する。
<#1EX=7TA=-9000TV=100TC=60TP=1000> 制御指令 (#1EX=OKTA=OKTV=OKTC=OKTP=OK)
これ以降角度のみを変化させるのであれば、角度指令のみを送信しても構わない。
<#1TA=1000>
各制御はPIDアンプを使用しているので、アプリケーションに見合ったゲインに調整した上で運用する事になる。その際に制御指令とゲインの他に現在値取得を追記し、そのパケットを繰り返し送信する事で現在値の変化を確認しながら調整を行う事になる。
<#1TA=5000SG0=2000;5000;0;0CA> (#TA=OKSG0=OKCA=3540) <#1TA=5000SG0=2000;5000;0;0CA> (#TA=OKSG0=OKCA=4302) <#1TA=5000SG0=2000;5000;0;0CA> (#TA=OKSG0=OKCA=4933) <#1TA=5000SG0=2000;5000;0;0CA> (#TA=OKSG0=OKCA=5010)
しかしこれを手動で行うのは現実的ではないため、後述する数値をグラフで表示するプログラム等を用意するのが良いだろう。
作者自身がパケットを手動で入力するのが面倒になり、テキストボックスに入力したパケットフレームを送るだけのWindowsアプリを作成。
任意のPortとBaudrateを選択してOpenを押せば準備完了。Transmission dataに送信パケットのテキストを入力しエンタキーを押すと送信され、サーボから応答があればReceived dataにその内容が表示される。またウィンドウを拡大すると、受信パケットの内容をコマンド毎に分解して表示する項目が見えるようになる。
NVMボタンは現在の設定を不揮発メモリに保存、INIボタンは完全初期化、任意のIDを指定して死活確認を行うPing、IDの変更(不揮発化あり)、baudrateの変更(不揮発化無し)する各機能を装備。
サーボのファームウェアはブートローダで紹介した操作によって書き換えができる。しかしTeraTermのマクロを使ってもそれ相応に煩雑な面があるので、本アプリのFirmware Update内にある3つのボタンでも書き換えができる(exeファイルと同一フォルダにJP200.binがある場合のみ使用可)。
2つ目のタブには4種類の波形で各制御値を更新させながら現在値をグラフで確認する機能を設けている。なお2つのタブは各々異なる方法で通信しているため、ポート名とボーレートは1つめタブの設定を共有するが、開閉は排他的になっている。
左上のスライドスイッチでポートの開閉、対象ID・制御モード・遷移時間・波形の周期・波形の種類、各制御の指令値とゲインといった条件や設定が簡易的に試せる。
プロジェクトのReleaseフォルダ内に「jp200_test.exe」として同梱済み。
teratermやjp200_testを使っても簡単なテキストによるパケットの送受信を確認するのが限界で、様々な値を試しながら調整を行うには役不足である。
そこで通信を行いながら各種の情報をリアルタイムにグラフ表示するアプリケーションを用意した。ボーレートやIDの変更・ID指定による検索・初期化や不揮発化・制御モードの選択・ダイアルや数値による各指令値やゲインの変更・任意波形による指令値の更新・モニタ値のスコープチャート表示・任意パケットの送信といった機能を備えている。
同梱のjp200_mon.exeはLabVIEWで作られているため、実行するには予めランタイムライブラリをインストールする必要がある。
パケットがテキストベースとは言え、シリアルターミナル上で手動による操作は限界がある。ここでは様々なプラットホームで使用できるPythonを使って通信する方法を紹介する。
ちなみに以下の例はRZboard+DXHATの構成をホストとして使用したものである。
まずパケットの送受信は以下のような感じでいかがだろうか。
ポートとタイムアウト[sec]を指定してpacketクラスを初期化。request関数は指定された条件のパケットを送信し、timeout時間まで「)」が受信されるのを待つ。受信データは「()」で括られた範囲のみを切り出す。複数のサーボを対象にしたパケットの場合はその数だけ受信を繰り返す。requestの引数にpackettype = UNILATERALを指定すると応答無しのパケットを送信、packettype = STRICTを指定するとCRCを付与して送受信。
おまけでモーションを再生するクラスも追加した。
#! python # # Communication process when using JP200 from python # # (C) 2023 T.Uemitsu import enum import serial, re, threading, time from collections import namedtuple #---------------------------------------------------- # Class for sending and receiving packets #---------------------------------------------------- # Level of packet robustness class PacketType(enum.Enum): CONVENIENCE = 0 UNILATERAL = 1 STRICT = 2 globals().update(PacketType.__members__) class packet: p0 = re.compile(r'(#+)=?([;0-9-]+)') p1 = re.compile(r'(?<=().+?(?=))') p2 = re.compile(r'(#|EX|[TC][AVCP]|ST|CT[0-2]|CB|SG[0-2]|INI|VER|RB|WD|WR|:|RR[0-9-]+)=?([;0-9-]+|OK|NG)') p3 = re.compile(r':[0-9-]+') p4 = re.compile(r':([0-9-]+)') __CRC8_SAE_J1850 = [0] * 256 def __init__(self, ser, timeout = 0.05): """Init jp200class.packet object. Parameters ---------- - serint (Serial): Initialized Serial object - timeout (float): Reception timeout time for one packet """ self.__serial = ser self.__serial.timeout = timeout self.__mutex = threading.Lock() for i in range(256): _crc = i for b in range(8): if _crc & 0x80 != 0: _crc = (_crc << 1) ^ 0x1D else: _crc = (_crc << 1) self.__CRC8_SAE_J1850[i] = _crc def __crc8(self, st): """Calculate CRC of specified string.""" _crc = 0xff for ch in st: _crc = self.__CRC8_SAE_J1850[(_crc ^ ord(ch)) & 0xff]; _crc = _crc ^ 0xff return _crc & 0xff def request(self, tx, rx = [], packettype = CONVENIENCE, echo = False): """Sending and receiving packets.""" self.__mutex.acquire() result = True if packettype == CONVENIENCE: tx = '<'+tx+'>' elif packettype == UNILATERAL: tx = '['+tx+']' elif packettype == STRICT: tx = '{'+tx+':'+str(self.__crc8(tx))+'}' if echo: print(tx) self.__serial.reset_input_buffer() self.__serial.write(tx.encode()) self.__serial.flush() if packettype != UNILATERAL: _num = 0 r = self.p0.findall(tx) if r: _num = len(set(r)) rx[:] = [] for i in range(_num): try: r = self.p1.findall(self.__serial.read_until(b')').decode("ascii", "ignore")) if r: if packettype == STRICT: s = self.p3.sub('',r[0]) if self.__crc8(s) == int(self.p4.findall(r[0])[0]): rx.append(r[0]) else: rx.append('') result = False else: rx.append(r[0]) else: result = False except: rx.append('') result = False if echo: print(rx) self.__mutex.release() return result #---------------------------------------------------- # Class for reproducing discretized trajectories #---------------------------------------------------- # Minimum Elements of Motion Pose = namedtuple ('Pose', ('mode', 'targetvalues', 'trajectory', 'time', 'option')) class motion: def __init__(self, pk, hp, maxth = 4): """Init jp200class.motion object. Parameters ---------- - pk (jp200class.packet): Initialized jp200class.packet object - hp (dict): Dict type summarizing ID(int) and initial angle(float) - maxth (int): Maximum number of threads """ self.__next_t = [time.time()] * maxth self.__motion = [None] * maxth self.__index = [0] * maxth self.__thread = [None] * maxth self.__pk = pk self.__hp = hp self.__IDs = [()] * maxth self.__maxth = maxth def __del__(self): for i in range(self.__maxth): self.stop(i) def __run(self, *arg): """Threads called for the first time and at every transition time""" no = arg[0] if self.__maxth > no: if len(self.__motion[no]) >= self.__index[no] + 1: m = self.__motion[no][self.__index[no]] acks = [] s = '' n = 0 for id, tval, h in zip(self.__IDs[no], m.targetvalues, self.__hp): if m.mode == 1 or m.mode == 3 or m.mode == 5 or m.mode == 7: s += ('#%dEX=%dTA=%d;%d;%d%s'%(id, m.mode, (tval + self.__hp[id]) * 100, m.trajectory, m.time, m.option)) elif m.mode == 2 or m.mode == 6: s += ('#%dEX=%dTV=%d;%d;%d%s'%(id, m.mode, tval, m.trajectory, m.time, m.option)) elif m.mode == 4: s += ('#%dEX=%dTC=%d;%d;%d%s'%(id, m.mode, tval, m.trajectory, m.time, m.option)) elif m.mode == 8: s += ('#%dEX=%dTP=%d%s'%(id, m.mode, tval, m.option)) else: s += ('#%dEX=%d%s'%(id, m.mode, m.option)) n += 1 self.__pk.request(s, acks, UNILATERAL) self.__next_t[no] += m.time / 1000.0 self.__index[no] += 1 if (self.__index[no] <= len(self.__motion[no])): self.__thread[no] = threading.Timer(self.__next_t[no] - time.time(), self.__run, [no]) self.__thread[no].start() else: self.__stop(no); def __stop(self, no): """Local end-of-timer processing.""" if self.__maxth > no: if self.__thread[no] != None: self.__thread[no].cancel() self.__thread[no] = None return True def stop(self, no): """Stop process including 'EX=14'.""" if self.__stop(no): s = '' for id in self.__IDs[no]: s += ('#%dEX=14'%(id)) self.__pk.request(s, packettype = UNILATERAL) def start(self, no, ids, motion): """Start of motion.""" if self.__stop(no): if len(motion) > 0: self.__IDs[no] = ids self.__motion[no] = motion self.__index[no] = 0 self.__next_t[no] = time.time() self.__run(no) def running(self, no): """Detects motion in progress.""" if self.__maxth > no: return self.__thread[no] != None else: return False
クラスを使って実際にパケットを送ってみる。request関数には括弧を除くパケットの中身を記述、受信データは文字列のlistで受け取る。ここではひとまずデバッグ目的でecho=Trueを指定。
import serial, time from jp200class import packet #シリアルポートを開く ser = serial.Serial('/dev/ttySC2', 1000000) pk = packet(ser, 0.02) #応答の受信用変数 acks = [] #角度制御のひな形 ang = '#%dEX=1TA=%dTP=1000' #ID=1へ180度の角度を指令 pk.request(ang%(1, 18000), acks, echo=True) time.sleep(0.5) #ID=1へ90度の角度を指令 pk.request(ang%(1, 9000), acks, echo=True) time.sleep(0.5) #ID=1へ270度の角度を指令 pk.request(ang%(1, 27000), acks, echo=True) ser.close()
実行するとターミナル上に以下の表示がなされる。
<#1EX=1TA=18000TP=1000> ['#1EX=OKTA=OKTP=OK'] <#1EX=1TA=9000TP=1000> ['#1EX=OKTA=OKTP=OK'] <#1EX=1TA=27000TP=1000> ['#1EX=OKTA=OKTP=OK'] >>>
次に受信したパケットを正規表現任せでコマンド毎に分解してみる。正規表現パターンはクラスに定義済みなのでそれを利用。
import serial from jp200class import packet ser = serial.Serial('/dev/ttySC2', 1000000) pk = packet(ser, 0.02) acks = [] #ID=1へ180度の角度指令と合わせて角度・角速度・電流・電源電圧を取得 pk.request('#1EX=1TA=18000TP=500CACVCCCB', acks) #応答が得られた時のみ正規表現でパケットを分離して表示 if acks: for ack in acks: m = pk.p2.findall(ack) for m in m: print(m) ser.close()
コマンドと結果がタプルとして表示される。
('#', '1') ('EX', 'OK') ('TA', 'OK') ('TP', 'OK') ('CA', '17792') ('CV', '0') ('CC', '0') ('CB', '12168') >>>
分離しただけでは意味が無いので、数値変換してID毎にリストに保存してみる。RRコマンドは';'をデリミタとしてデータが連なって返ってくるのでsplitを使って更に分割。
import serial import jp200class as jp200 ser = serial.Serial('/dev/ttySC2', 1000000) pk = jp200.packet(ser, 0.02) acks = [] #軸数分の情報を保存するリスト CurrentValues = [[0] * 15] * 2; #ID=1とID=2のコントロールテーブルの45から15個のデータを読み出し if pk.request('#1RR45;15#2RR45;15', acks): # 軸数分ループ for ack in acks: # コマンド毎に分離 r = pk.p2.findall(ack) if r: ind = -1 # コマンド数分ループ for _r in r: if _r[0] == '#' and int(_r[1]) <= 2: ind = int(_r[1]) - 1 if _r[0] == 'RR45' and ind >= 0: CurrentValues[ind] = [int(s) for s in _r[1].split(';')] # 取得した現在値の一部を表示 print ('[0] {0[0]:6} {0[1]:5} {0[2]:5}{0[12]:10} [1] {1[0]:6} {1[1]:5} {1[2]:5} {1[12]:10}'.format(*CurrentValues)) ser.close()
[0] 27041 0 -19 1211739 [1] 17892 0 0 1211737 >>>
モーションは時間を追って変化する目標値を任意の時間で区切って羅列したデータを予め作っておき、そのデータをサーボの補間機能を活用しながら再生するもの。ここでは2個のサーボを角度制御と速度制御によるモーションを使って運転してみる。
import serial import jp200class as jp200 portname = '/dev/ttySC2' #portname = '\\.\COM12' baudrate = 1000000 HomeOffsetAngle = {1:180.0, 2:180.0} # Target ID IDs = (1,2) # Motion data m0 = ( jp200.Pose(1, [ 0.0, 0.0 ], 1, 1000, 'TP=1000'), jp200.Pose(1, [ 90.0, 10.0 ], 1, 1500, ''), jp200.Pose(1, [ 180.0, 20.0 ], 1, 1500, ''), jp200.Pose(1, [ -90.0, -30.0 ], 1, 1500, ''), ) m1 = ( jp200.Pose(1, [ 0.0, 0.0 ], 1, 1000, 'TP=1000'), jp200.Pose(1, [ 200.0, 200.0 ], 1, 500, ''), jp200.Pose(1, [ 100.0, 100.0 ], 1, 500, ''), jp200.Pose(1, [-100.0,-100.0 ], 1, 500, ''), jp200.Pose(1, [ 0.0, 0.0 ], 1, 500, ''), ) ser = serial.Serial(portname, baudrate) pk = jp200.packet(ser, 0.02) mt = jp200.motion(pk, HomeOffsetAngle) s = '' while s != 'q': s = input('>') if s == '1': mt.start(0, IDs, m0) if s == '2': mt.start(0, IDs, m1) if s == '3': mt.start(0, IDs, (jp200.Pose(2, [ 400.0, 800.0 ], 1, 500, 'TP=1000'),)) if s == '4': mt.start(0, IDs, (jp200.Pose(2, [ -800.0, -400.0 ], 1, 500, 'TP=1000'),)) if s == '5': mt.start(0, IDs, (jp200.Pose(2, [ 0.0, 0.0 ], 1, 500, 'TP=1000'),)) if s == 'e': print('stop') for i in range(4):mt.stop(i) for i in range(4):mt.stop(i) del mt, pk ser.close()
最後にもう一つ。サーボのIDやボーレートを変更する場合はjp200_testで行えるが、Pythonでも変更するコードを紹介しておく。
import time, sys, serial, re, os from jp200class import packet hostbaud = 1000000 baudlist = [9600, 19200, 38400, 57600, 100000, 115200, 200000, 230400, 460800, 500000, 1000000, 2000000, 3000000, 4000000] term = False def hlp(v): print(' id CurrentID ChangedID : Change targett ID') print(' baud [baudrate | ID baudrate] : Change host or target baudrate') print(' ping ID : Check response for current ID') print(' scan : Scan all id ranges') print(' scan2 : Scan all baudrate and id ranges') print(' nvm ID : Make settings non-volatile') print(' quit : Quit') return True def bye(v): print("Bye!"); global term term = True return True def _ping(id): ack = [] pk.request('#%d'%(id), ack) if ack: return '#%d'%(id) in ack[0] return False def Ping(v): print(" Ping") if len(v) == 2: if v[1].isdecimal(): if _ping(int(v[1])): print (" Found %d"%(int(v[1]))) else: print (" Not found %d"%(int(v[1]))) else: print(" missing argument !!") return False def Scan(v): detect = False print(" Scan") if len(v) == 1: try: for id in range(0, 254): if _ping(id): detect = True print('(%d)'%(id), end='', flush = True) else: print('.', end='', flush = True) print() except KeyboardInterrupt: print(); else: print(" missing argument !!") return detect def Scan2(v): detect = False print(" Scan2") if len(v) == 1: try: for b in baudlist: print ('baudrate=',b); ser.baudrate = b for id in range(0, 254): if _ping(id): detect = True print('(%d)'%(id), end='', flush = True) else: print('.', end='', flush = True) print() except KeyboardInterrupt: print(); else: print(" missing argument !!") return detect def ChangeTargetID(v): global hostid print(" Change target ID") if len(v) == 3: if v[1].isdecimal(): curid = int(v[1]) if v[2].isdecimal(): changedid = int(v[2]) if changedid <= 254: if _ping(curid) and not _ping(changedid): ack = [] pk.request( '#%dWR0=%d'%(curid, changedid), ack) if ack: if '#%dWR=OK'%(curid) in ack[0] and _ping(changedid): #hostid = changedid print(" Changed ID %d to %d."%(curid, changedid)) return True print(" Fail to change") else: print(" missing argument !!") return False def ChangeTargetBaud(v): global hostbaud if len(v) == 2: print(" Change host baudrate"); if v[1].isdecimal(): if int(v[1]) in baudlist: ser.baudrate = int(v[1]) hostbaud = int(v[1]) return True elif len(v) == 3: print(" Change target baudrate"); if v[1].isdecimal(): id = int(v[1]) if v[2].isdecimal(): changedbaud = int(v[2]) if changedbaud in baudlist: baudind = baudlist.index(changedbaud) ser.baudrate = changedbaud if not _ping(id): ser.baudrate = hostbaud ack = [] pk.request( '#%dWR1=%d'%(id, baudind), ack) if ack: if '#%dWR='%(id) in ack[0]: ser.baudrate = changedbaud if _ping(id): hostbaud = changedbaud return True else: print(" missing argument !!") return False def WriteNVM(v): global hostid print(" WriteNVM") if len(v) == 2: if v[1].isdecimal(): id = int(v[1]) ack = [] pk.request( '#%dWD'%(id), ack) if ack: print(ack) return '#%dWD=OK'%(id) in ack[0] else: print(" missing argument !!") return False cmdlist = {"help":hlp, "?":hlp, "quit":bye, "q":bye, "ping":Ping, "scan":Scan, "scan2":Scan2, "id": ChangeTargetID, "baud": ChangeTargetBaud, "nvm": WriteNVM} try: ser = serial.Serial('\\.\COM12', hostbaud) #ser = serial.Serial('/dev/ttySC2', hostbaud, timeout = 0.02) print (ser.name) pk = packet(ser, 0.02) k = '' cmd = [] while not term: k = input('%dbps>'%(hostbaud)) cmd = k.split(' ') if cmd[0] in cmdlist: result = cmdlist[cmd[0]](cmd) ser.close() except serial.serialutil.SerialException: pass os._exit(0)
XH540-V150 | 1188 | 0~1188 |
XM540-W270 | 2047 | 0~2047 |
XH540-V270 | 1188 | 0~1188 |
XW540-T140 | 2047 | 0~2047 |
Operating ModeにPWM Modeが設定されている事はもとより、全てのModeにおける制御演算結果はPWMのデューティ比として算出されるため、必ず制御の最終段においてこの値以下にデューティー比が制限されモータへ印加されます。
¶XL330-M077 | 445 | 0~2047 |
XL330-M288 | 1620 |
XL330-M077 | 1750 | 0~1750 | 1 |
XL330-M288 |
XL330-M077 | 70 | 35 | 31~70 |
XL330-M288 |
ファームウェア Ver.44以降を搭載した以下のDynamixelに共通するコントロールテーブルです。
Address | Item | Access | Default Value | Type/Range |
0 | Model Number | R | - | uint16 |
1 | ||||
2 | Model Information | R | 0 | uint32 |
3 | ||||
4 | ||||
5 | ||||
6 | Version of Firmware | R | ? | uint8 |
7 | ID | R/W (NVM) | 1 | uint8 0~252 |
8 | Baudrate | R/W (NVM) | 1 | uint8 |
9 | Return Delay Time | R/W (NVM) | 250 | uint8 0~254 |
10 | Drive Mode | R/W (NVM) | 0 | uint8 0~255 |
11 | Operating Mode | R/W (NVM) | 3 | uint8 0~16 |
12 | Secondary(Shadow) ID | R/W (NVM) | 255 | uint8 0~255 |
13 | Protocol Version | R/W (NVM) | 2 | uint8 |
14 ~ 19 | (reserve) | R | - | uint8 |
20 | Homing Offset | R/W (NVM) | 0 | int32 -1044479~1044479 |
21 | ||||
22 | ||||
23 | ||||
24 | Moving Threshold | R/W (NVM) | 10 | uint32 0~1023 |
25 | ||||
26 | ||||
27 | ||||
28 ~ 30 | (reserve) | R | - | uint8 |
31 | Temperature Limit | R/W (NVM) | - | uint8 0~100 |
32 | Max Voltage Limit | R/W (NVM) | - | uint16 |
33 | ||||
34 | Min Voltage Limit | R/W (NVM) | - | uint16 |
35 | ||||
36 | PWM Limit | R/W (NVM) | 885 | uint16 0~885 |
37 | ||||
38 | Current Limit | R/W (NVM) | - | uint16 |
39 | ||||
40 | Acceleration Limit | R/W (NVM) | 32767 | uint32 0~32767 |
41 | ||||
42 | ||||
43 | ||||
44 | Velocity Limit | R/W (NVM) | - | uint32 |
45 | ||||
46 | ||||
47 | ||||
48 | Max Position Limit | R/W (NVM) | 4095 | uint32 0~4095 |
49 | ||||
50 | ||||
51 | ||||
52 | Min Position Limit | R/W (NVM) | 0 | uint32 0~4095 |
53 | ||||
54 | ||||
55 | ||||
56 | External Port Mode 1 | R/W (NVM) | 3 | uint8 0~3 |
57 | External Port Mode 2 | R/W (NVM) | 3 | uint8 0~3 |
58 | External Port Mode 3 | R/W (NVM) | 3 | uint8 0~3 |
59 ~ 62 | (reserve) | R | - | uint8 |
63 | Shutdown | R/W (NVM) | - | uint8 0~63 |
64 | Torque Enable | R/W | 0 | uint8 0~1 |
65 | LED | R/W | 0 | uint8 0~1 |
66 ~ 67 | (reserve) | R | - | uint8 |
68 | Status Return Level | R/W | 2 | uint8 0~2 |
69 | Registered Instruction | R | 0 | uint8 |
70 | Hardware Error Status | R | 0 | uint8 |
71 ~ 75 | (reserve) | R | - | uint8 |
76 | Velocity I Gain | R/W | - | uint16 0~16383 |
77 | ||||
78 | Velocity P Gain | R/W | - | uint16 0~16383 |
79 | ||||
80 | Position D Gain | R/W | - | uint16 0~16383 |
81 | ||||
82 | Position I Gain | R/W | - | uint16 0~16383 |
83 | ||||
84 | Position P Gain | R/W | - | uint16 0~16383 |
85 | ||||
86 ~ 87 | (reserve) | R | - | uint8 |
88 | Feedforward Acceleration Gain | R/W | 0 | uint16 0~16383 |
89 | ||||
90 | Feedforward Velocity Gain | R/W | 0 | uint16 0~16383 |
91 | ||||
92 ~ 97 | (reserve) | R | - | uint8 |
98 | Bus Watchdog | R/W | 0 | int8 -1~127 |
99 | (reserve) | R | - | uint8 |
100 | Goal PWM | R/W | - | int16 -PWM Limit~PWM Limit |
101 | ||||
102 | Goal Current | R/W | - | int16 -Current Limit~Current Limit |
103 | ||||
104 | Goal Velocity | R/W | - | int32 -Velocity Limit~Velocity Limit |
105 | ||||
106 | ||||
107 | ||||
108 | Profile Acceleration | R/W | 0 | uint32 0~32767 |
109 | ||||
110 | ||||
111 | ||||
112 | Profile Velocity | R/W | 0 | uint32 0~32767 |
113 | ||||
114 | ||||
115 | ||||
116 | Goal Position | R/W | - | int32 Min Position Limit~Max Position Limit -1048575~1048575 |
117 | ||||
118 | ||||
119 | ||||
120 | Realtime Tick | R | - | uint16 |
121 | ||||
122 | Moving | R | 0 | uint8 |
123 | Moving Status | R | 0 | uint8 |
124 | Present PWM | R | - | int16 |
125 | ||||
126 | Present Current/Present Load | R | - | int16 |
127 | ||||
128 | Present Velocity | R | - | int32 |
129 | ||||
130 | ||||
131 | ||||
132 | Present Position | R | - | int32 |
133 | ||||
134 | ||||
135 | ||||
136 | Velocity Trajectory | R | - | |
137 | ||||
138 | ||||
139 | ||||
140 | Position Trajectory | R | - | |
141 | ||||
142 | ||||
143 | ||||
144 | Present Input Voltage | R | - | uint16 |
145 | ||||
146 | Present Temperature | R | - | uint8 |
147 ~ 151 | (reserve) | R | - | uint8 |
152 | External Port Data 1 | R/W | - | uint16 |
153 | ||||
154 | External Port Data 2 | R/W | - | uint16 |
155 | ||||
156 | External Port Data 3 | R/W | - | uint16 |
157 | ||||
158 ~ 167 | (reserve) | R | - | uint8 |
168 | Indirect Address 1 | R/W | 224 | uint16 64~661 |
169 | ||||
170 | Indirect Address 2 | 225 | ||
171 | ||||
172,173 ~ 218,219 | Indirect Address 3 ~ Indirect Address 26 | 226 ~ 249 | ||
220 | Indirect Address 27 | 250 | ||
221 | ||||
222 | Indirect Address 28 | 251 | ||
223 | ||||
224 | Indirect Data 1 | R/W | 0 | uint8 |
225 | Indirect Data 2 | |||
226 | Indirect Data 3 | |||
227 ~ 248 | Indirect Data 4 ~ Indirect Data 25 | |||
249 | Indirect Data 26 | |||
250 | Indirect Data 27 | |||
251 | Indirect Data 28 | |||
252 ~ 577 | (reserve) | R | - | uint8 |
578 | Indirect Address 29 | R/W | 634 | uint16 64~661 |
579 | ||||
580 | Indirect Address 30 | 635 | ||
581 | ||||
582,583 ~ 628,629 | Indirect Address 31 ~ Indirect Address 54 | 636 ~ 659 | ||
630 | Indirect Address 55 | 660 | ||
631 | ||||
632 | Indirect Address 56 | 661 | ||
633 | ||||
634 | Indirect Data 29 | R/W | 0 | uint8 |
635 | Indirect Data 30 | |||
636 ~ 659 | Indirect Data 31 ~ Indirect Data 54 | |||
660 | Indirect Data 55 | |||
661 | Indirect Data 56 |
モデル固有の値を保持します。異なる種類のDynamixelを混在して使用する際の個体識別などに使用できます。
Model Name | Value |
XC330-M077 | 1190(0x4A6) |
XC330-M288 | 1200(0x4B0) |
XC330-M181 | 1200(0x4B0) |
XC330-M288 | 1240(0x4D8) |
XC330-T181 | 1210(0x4BA) |
XC330-T288 | 1220(0x4C4) |
XL430-W250 | 1230(0x4CE) |
2XL430-W250 | 1090(0x442) |
XC430-W150 | 1070(0x42E) |
XC430-W250 | 1080(0x438) |
2XC430-W250 | 1160(0x488) |
XM430-W210 | 1030(0x406) |
XH430-W210 | 1010(0x3F2) |
XH430-V210 | 1050(0x41A) |
XD430-T210 | 1011(0x3F3) |
XM430-W350 | 1020(0x3FC) |
XH430-W350 | 1000(0x3E8) |
XH430-V350 | 1040(0x410) |
XD430-T350 | 1001(0x3E9) |
XW430-T200 | 1280(0x500) |
XW430-T333 | 1270(0x4F6) |
XM540-W150 | 1130(0x46A) |
XH540-W150 | 1110(0x456) |
XH540-V150 | 1150(0x47E) |
XM540-W270 | 1120(0x460) |
XH540-W270 | 1100(0x44C) |
XH540-V270 | 1140(0x474) |
XW540-T140 | 1180(0x49C) |
XW540-T260 | 1170(0x492) |
各Dynamixelを特定するための固有の値で0~252の範囲の数値で設定します。同一ネットワーク内に存在するDynamixelには各々異なるIDが要求されます。
なお2XLや2XCシリーズは1つのデバイスであってもネットワーク上では2個のDynamixelとみなされるため、設定しようとする対象のID番号はもう一方のID番号と同じ値を設定することができません。
通信する際のボーレートです。ホストとDynamixelのボーレートは一致させなくてはなりません。
Value | Baudrate [bps] |
0 | 9600 |
1 | 57600 |
2 | 115200 |
3 | 1000000 |
4 | 2000000 |
5 | 3000000 |
6 | 4000000 |
7 | 4500000 |
インストラクションパケットが送られた後、ステータスパケットを返すまでの待ち時間を設定します。
ホストにおいて半二重のバス制御のタイミングに合わせて調整しますが、弊社が提供するPC用USBシリアルI/Fを使用する限りでは0を設定しても問題ありません。
Delay Time [us] = Value * 2 [us]
デフォルト回転方向、デュアルジョイント、プロファイル構成を設定します。
デフォルト回転方向によりはPosition, Velocity, PWMの各指令によるホーンの回転方向が変化します。
デュアルジョイントは2台のDynamixelを同期させてトルクアップする際に使用し、一方をマスターもう一方をスレーブに設定します。※X540シリーズのみ装備
プロファイル構成は位置決め制御時に速度制御を行うか遷移時間制御を行うかを選択します。
Bit | Name | Description |
7 | 常時0 | |
6 | 常時0 | |
5 | 常時0 | |
4 | 常時0 | |
3 | 常時0 | |
2 | Profile configuration | 0:Velocity-based Profile 1:Time-based Profile |
1 | Dual Joint | 0:Master 1:Slavebr;※X540シリーズのみ |
0 | Direction of rotation | 0:Normal (CCW方向を+、CW方向を-) 1:Reverse (CCW方向を-、CW方向を+) |
2台のDynamixelのDual Jointを各々MasterとSlaveに設定し、Dual Joint Connector同士をデュアルジョイントケーブルで接続します。Slaveに設定されたDynamixelはDual Joint Connectorから入力されるMasterからの信号でのみ動作し、Slaveへの制御モード等の設定は一切無視されます。なお、ケーブルの配線方法により回転方向が決定されます。
Dual Joint | |
Master | Slave |
動作モードを選択します。Valueに記載が無い値は予約済みのため、指定してはなりません。
なお、電流センサを搭載しないデバイスは一部のモードが選択できません。
Value | Mode | Description |
0 | Current Control Mode | 電流制御。位置及び速度制御は行わない。 ※XL430,2XL,XC,2XCシリーズは指定不可 |
1 | Velocity Control Mode | 速度制御。位置及びトルク制御は行わない。 |
3 | Position Control Mode | 位置制御。GoalPositionは0~360°の1回転分の制御範囲に制限。 |
4 | Extended Position Control Mode | 拡張位置制御。GoalPositionの範囲が拡大され、最大±256回転まで対応。 |
5 | Current-Base Position Control Mode | 電流制限付き位置制御。GoalPositionの範囲が拡大され、最大±256回転まで対応。 ※XL430,2XL,XC,2XCシリーズは指定不可 |
16 | PWM Control Mode | PWMのデューティー比を制御。 |
DynamixelのSecondary IDを設定します。Secondary IDは、IDと同様に各Dynamixelを識別するために用いられます。なお、Secondary IDに253以上の値が設定されている場合、Secondary IDは機能しません。
プロトコルを選択します。他のシリーズのDynamixelと混在させて使用する際はそのプロトコルに合わせますが、異なるプロトコルを混在させて使用する事はできません。
Value | Protocol | Description |
1 | DXL 1.0 | AX, DX, RX, MX, EXシリーズと互換 ※XL330,XC330シリーズは指定不可 |
2 | DXL 2.0 | X, PROシリーズと互換 |
20 | S.BUS | Futaba S.BUS (試行的実装) ※XL330,XC330シリーズのみ指定可 |
21 | iBUS | FlySky (試行的実装) ※XL330,XC330シリーズのみ指定可 |
22 | RC-PWM | PWM ※XL330,XC330シリーズのみ指定可 |
この値が真の現在位置に加算されPresent Positionに反映されます。オフセット位置をホスト側では無くDynamixel側に持たせる際に使用します。
真の現在位置とはMulti Turnがoff、Homing Offsetが0、Direction of rotationが0の時のPresent Positionを意味します。
Position [deg] = Value * 360 [deg] / 4095
Present Temperatureがこの値を超えるとHardware Error Statusの該当ビットがONになり、Shutdownで指定された動作に遷移します。
Temperature [degC] = Value * 1 [degC]
Model | Default value |
XL330-M077 | 70 |
XL330-M088 | |
XC330-M181 | |
XC330-M288 | |
XC330-T181 | |
XC330-T288 | |
XL430-W250 | 72 |
2XL430-W250 | |
XC430-W150 | 80 |
XC430-W240 | |
2XC430-W250 | |
XM430-W210 | |
XH430-W210 | |
XH430-V210 | |
XD430-T210 | |
XM430-W350 | |
XH430-W350 | |
XH430-V350 | |
XD430-T350 | |
XW430-T200 | |
XW430-T333 | |
XM540-W150 | |
XH540-W150 | |
XH540-V150 | |
XM540-W270 | |
XH540-W270 | |
XH540-H270 | |
XW540-T140 | |
XW540-T260 |
Present Input Voltageがこの値の範囲を超えるとHardware Error Statusの該当ビットはONになり、Shutdownで指定された動作に遷移します。
Voltage [V] = Value * 0.1 [V]
Model | Default value | Range | |
Max | Min | ||
XL330-M077 | 70 | 35 | 31~70 |
XL330-M288 | |||
XL430-W250 | 140 | 60 | 60~140 |
2XL430-W250 | |||
XC430-W150 | 160 | 60 | 60~160 |
XC430-W240 | |||
2XC430-W250 | |||
XM430-W210 | 160 | 95 | 95~160 |
XH430-W210 | |||
XM430-W350 | |||
XH430-W350 | |||
XM540-W150 | |||
XH540-W150 | |||
XM540-W270 | |||
XH540-W270 | |||
XW540-T140 | |||
XW540-T260 | |||
XH430-V210 | 300 | 110 | 110~300 |
XH430-V350 | |||
XH540-V150 | |||
XH540-V270 |
Goal Currentの絶対値はこの値以下に制限されます。
Current [mA] = Value * CurrentScalingFactor [mA]
Model | Default value | Range | Current Scaling factor [mA] |
XL330-M077 | 1750 | 0~1750 | 1 |
XL330-M288 | |||
XL430-W250 | - | - | - |
2XL430-W250 | |||
XC430-W150 | |||
XC430-W240 | |||
2XC430-W250 | |||
XM430-W210 | 1193 | 0~1193 | 2.69 |
XH430-W210 | 648 | 0~648 | |
XM430-W350 | 1193 | 0~1193 | |
XH430-W350 | 648 | 0~648 | |
XM540-W150 | 2047 | 0~2047 | |
XH540-W150 | |||
XH540-V150 | 1188 | 0~1188 | |
XM540-W270 | 2047 | 0~2047 | |
XH540-W270 | |||
XH540-V270 | 1188 | 0~1188 | |
XW540-T140 | 2047 | 0~2047 | |
XW540-T260 | |||
XH430-V210 | 689 | 0~689 | 1.34 |
XH430-V350 | 689 | 0~689 |
Profile Accelerationはこの値以下に制限されます。
Acceleration [rpm²] = Value * 214.577
XL330シリーズでは指定できません。
Goal Velocityの絶対値とProfile Velocityはこの値以下に制限されます。
Velocity [rpm] = Value * 0.229 [rpm]
Model | Default value | Range |
XL330-M077 | 445 | 0~2047 |
XL330-M288 | 1620 | |
XL430-W250 | 265 | 0~1023 |
2XL430-W250 | 250 | |
XC430-W150 | 460 | |
XC430-W240 | 306 | |
2XC430-W250 | 275 | |
XM430-W210 | 330 | |
XH430-W210 | 210 | |
XH430-V210 | 230 | |
XM430-W350 | 200 | |
XH430-W350 | 130 | |
XH430-V350 | 135 | |
XM540-W150 | 230 | |
XH540-W150 | 300 | |
XH540-V150 | 230 | |
XM540-W270 | 128 | |
XH540-W270 | 167 | |
XH540-V270 | 128 | |
XW540-T140 | 304 | |
XW540-T260 | 167 |
Operating ModeにPosition Control Modeが設定されている時にGoal Positionはこの値の範囲内に制限されます。
Position [deg] = Value * 360 [deg] / 4096
Modeにて3つのExternal Portをデジタル入出力、もしくはアナログ入力に設定します。
※X540シリーズのみ装備
Mode Value | Mode | Description |
0 | Analog IN | PortNへ入力された0~3.3Vの電圧を12bitの分解能でA/D測定しDataNにストア |
1 | Digital OUT (PushPull) | DataNに0の書き込みでPortNから3.3V、1の書き込みでPortNから0Vを出力 VOH:2.4V, VOL:0.5V |
2 | Digital IN (PullUp) | PortNへ0Vの入力でDataに0、PortNへ3.3Vの入力でDataに1をストア VIH:2.3V, VIL:1.0V PullUp/Down Reg:40kΩ(typ) |
3 | Digital IN (PullDown) |
この設定とHardware Error Statusの論理積が0以外になると、Torque Enableは0になりモータの出力が遮断されシャットダウン状態に遷移します。以後通常のインストラクションパケットにてTorque Enableを1にする事ができません。
Bit | Name |
7 | 常時0 |
6 | 常時0 |
5 | Overload Error |
4 | Electrical Shock Error |
3 | Motor Encoder Error |
2 | Overheating Error |
1 | 常時0 |
0 | Input Voltage Error |
なお、シャットダウン状態から復帰するには発生している障害を排除した後、電源の再投入か、REBOOTインストラクションパケットを受信しなくてはなりません。
出力軸をフリーにするか、設定されたOperating Modeに従った制御を開始します。
Value | Description |
0 | 出力軸フリー、制御停止、ロックされたアイテムを解除 |
1 | Operating Modeに従った制御開始、NVM及びIndirect Address領域のアイテムロック |
ステータスパケットを返信するインストラクションパケットを選択します。
Value | Instruction to respond |
0 | Ping |
1 | Ping, Read |
2 | Ping, Read, Write, Reg Write, Factory Reset, Reboot, Sync Read, Bulk Read |
様々なフィードバックと内部の制御状態を比較した結果を示します。さらに、この値とShutdownの論理積の結果により動作を継続するか否かを決定します。
Bit | Name | Description |
7 | - | 常時0 |
6 | - | 常時0 |
5 | Overload Error | 最大出力で制御できない負荷が継続的に発生した |
4 | Electrical Shock Error | 電気的に回路が衝撃を受けたり入力電力が不足してモータが正常動作しない |
3 | Motor Encoder Error | エンコーダが正常動作しない |
2 | Overheating Error | Present TemperatureがTemperature Limitを超えた |
1 | - | 常時0 |
0 | Input Voltage Error | Present VoltageがMax/Min Voltage Limitの範囲を超えた |
速度制御演算における各種制御ゲインを指定します。
Operating ModeにVelocity Control Modeが設定されている時に有効です。
なお、制御ブロック中の各ゲインは、次の算式で示す減じられた値が用いられます。
KvI = (Velocity I Gain) / 65536 KvP = (Velocity P Gain) / 128
位置制御演算における各種制御ゲインを指定します。
Operating ModeにPosition Control Mode・Extended Position Control Mode・Current-Base Position Control Modeが設定されている時に有効な値です。
なお、制御ブロック中の各ゲインは、次の算式で示す減じられた値が用いられます。
KpD = (Position I Gain) / 16 KpI = (Position I Gain) / 65536 KpP = (Position P Gain) / 128
Model | Default Value | ||||
Velocity Gain | Position Gain | ||||
I | P | D | I | P | |
XL430-W250 | 1000 | 100 | 4000 | 0 | 640 |
2XL430-W250 | 1800 | 100 | 2000 | 0 | 640 |
XC430-W150 | 1920 | 100 | 0 | 0 | 460 |
XC430-W240 | 1920 | 100 | 0 | 0 | 700 |
2XC430-W250 | 1920 | 100 | 0 | 0 | 700 |
XM430-W210 | 1920 | 100 | 0 | 0 | 800 |
XH430-W210 | 1920 | 100 | 0 | 0 | 900 |
XH430-V210 | 1920 | 100 | 0 | 0 | 800 |
XM430-W350 | 1920 | 100 | 0 | 0 | 800 |
XH430-W350 | 1920 | 100 | 0 | 0 | 900 |
XH430-V350 | 1920 | 100 | 0 | 0 | 800 |
XM540-W150 | 1920 | 100 | 0 | 0 | 800 |
XH540-W150 | 1920 | 100 | 0 | 0 | 800 |
XH540-V150 | 1920 | 100 | 0 | 0 | 800 |
XM540-W270 | 1920 | 100 | 0 | 0 | 800 |
XH540-W270 | 1920 | 100 | 0 | 0 | 800 |
XH540-V270 | 1920 | 100 | 0 | 0 | 800 |
XW540-T140 | 1920 | 100 | 0 | 0 | 800 |
XW540-T260 | 1920 | 100 | 0 | 0 | 800 |
無通信状態を監視する時間を指定します。
Bus Watchdogが1以上でかつTorque Enableが1である場合、ホストコントローラとDynamixel間の通信間隔を監視します。その間隔が指定時間よりも大きい場合にDynamixelは停止し、Bus Watchdogは-1に変更されます。Bus Watchdog Error状態になると、Goal PWM・Goal Current・Goal Velocity・Goal Positionの各アイテムは読み取り専用に変更されます。Bus Wathdogの値を0に変更すると、Bus Watchdog Errorは解除されます。
Value | Description |
0 | Bus Watchdog無効, Bus Watchdog Error状態を解除 |
1~127 | Bus Watchdog有効 (Value * 20[ms]) |
-1 | Bus Watchdog Error状態 |
PWMのデューティー比を指定します。
Operating ModeにPWM Modeが設定されている事はもとより、全てのModeにおける制御演算結果はPWMのデューティ比として算出されるため、必ず制御の最終段においてこの値以下にデューティー比が制限されモータへ印加されます。
Duty [%] = Value * 100 [%] / 855
電流センサを搭載したモデルにおいて電流制御の目標値を指定します。
Operating ModeにCurrent Control ModeもしくはCurrent-Base Position Control Modeが設定されている時に有効な値で、Current-Base Position Control Mode時はPosition D/I/P Gain, Feedforward 2nd/1st Gainに示すブロック図に従って制御されます。
Current [mA] = Value * ScalingFactor [mA]
モデル毎のCurrent Scaling Factorはこちら。
速度制御の目標値を指定します。
Operating ModeにVelocity Control Modeが設定されている時に有効な値で、Velocity I/P Gaiに示すブロック図に従って制御されます。
Velocity [rpm] = Value * 0.229 [rpm]
Profileの加速度もしくは加速時間を指定します。
Operating ModeがCurrent Control Modeである時を除くModeに有効です。
Drive ModeのProfile ConfigurationがVelocity-basedの場合、加速度は以下の式で決まります。
Acceleration [rpm²] = Value * 214.577
詳細はProfile Velocityを参照ください。なお0はモータの最大能力で加速します。
Drive ModeのProfile ConfigurationがTime-basedの場合、Profile AccelerationがProfile Velocityの50%を超えると50%に制限されます。
Velocity [rpm] = Value * 0.229 [rpm]
Profile | Condition, Waveform |
ステップ | Velocity = 0, Acceleration = don't care |
矩形 | Velocity ≠ 0, Acceleration = 0 |
三角 | Velocity ≠ 0, Acceleration ≠ 0 |
台形 | Velocity ≠ 0, Acceleration ≠ 0 |
t1 [ms] = 64 * (Profile Velocity) / (Profile Acceleration) もしくは t1 [ms] = 64 * (Goal Velocity) / (Profile Acceleration) t2 [ms] = 64 * Δ(Present Position) / (Profile Velocity)
また、これらの他に急峻な加速度変化を抑える制御を行っているため、最終的な目標到達時間はt3よりも長くなる場合があります。
Time [ms] = Value
位置制御の目標値を指定します。
Operating ModeにPosition Control Mode・Extended Position Control Mode・Current-Base Position Control Modeが設定されている時に有効で、各Mode毎に指摘できる数値範囲が異なります。
Operating Mode | Value Range | Max Turnover Number |
3 | Min Position Limit~Max Position Limit | 1 |
4 | -1048575~+1048575 | -256~+256 |
5 |
Position [deg] = Value * 360 / 4096
Present Velocityの絶対値とMoving Thresholdの比較結果を示します。
Value | Description |
0 | Moving Threshold ≥ |Present Velocity| |
1 | Moving Threshold < |Present Velocity| もしくはProfileが進行中 |
動作中の状況を示します。
Bit | Name | Description |
7 | - | 常時0 |
6 | - | 常時0 |
5 | Profile Type | 11:台形速度Profile 10:三角速度Profile 01:矩形速度Profile 00:Step速度Profile |
4 | ||
3 | Following Error | 位置制御時、位置がProfileに非追従 |
2 | - | 常時0 |
1 | Profile Ongoing | Goal Positionに基づくProfile進行中 |
0 | In-Position | 位置制御時、目標位置到達 |
Present Currentは現在モータへ流れている電流で、電流センサを搭載したモデルのみ有効です。
Current [mA] = Value * ScalingFactor [mA]
モデル毎のCurrent Scaling Factorはこちら。
電流センサを搭載しないモデルの場合はPresent Loadが適用され、最大トルクに対する負荷の割合を意味します。
Load [%] = Value * 0.1
センサ等を用いない推定値のため、負荷の方向を知る程度の目的に使用して下さい。
真の位置からHoming Offsetを除した出力軸の位置です。
Torque Enableが0の状態ではレンジフローするまで回転に応じた増減をしますが、Torque Enableを1にした瞬間にOperating Modeに依存した値でクリップされます。
Position [deg] = Value * 360 [deg] / 4096
Profileによって生成された目標速度を逐次示します。
Operating ModeにVelocity Control Mode・Position Control Mode・Extended Position Control Mode・Current-Base Position Control Modeが設定されている時に有効です。
Profileによって生成された目標位置を逐次示します。
Operating ModeにPosition Control Mode・Extended Position Control Mode・Current-Base Position Control Modeが設定されている時に有効です。
コントロールテーブル上のアドレスを再構成します。
Indirect Address N(N=1~56)とIndirect Data N(N=1~56)は対になっており、Indirect Address Nに任意のコントロールテーブル上のアドレスXを設定すると、その後Indirect Data Nへアクセスする事はIndirect Address Nに設定されたアドレスXへ間接的にアクセスする事になります。
具体的な例として、ホストから頻繁にアクセスしたいXM430のアイテムが複数あり、それらが離れたアドレスに配置されていた場合を考えます。通常はそれらアイテム全てをまたぐアドレス範囲のデータをまとめてアクセスするか、個々のアイテムに個別にアクセスするとった手段を執ります。これには本来無用なデータやアクセス回数が強いられるため、通信のトラフィックが上がる要因になるのと、ホスト側のプログラムの負担になります。
ホストから書き込み対象としてPosition P Gain・Goal Velocity・Goal Position、読み出し対象としてPresent Position・Present Temperatureがあった場合、これらをIndirect Data領域に再配置するには以下の手順を踏みます。
以後Indirect Data 1からの連続した224番地にアクセスする事は、再配置したアイテムへ個々に間接的にアクセスした事になります。
Indirect Addressがデフォルト値のままであれば、Indirect Data領域はユーザ任意のRAM領域として扱うことができます。
なお、XL330シリーズは本領域が以下のとおり縮小されていますので注意が必要です。
XL330 series only | ||||
Address | Item | Access | Default Value | Type/Range |
168 | Indirect Address 1 | R/W | 208 | uint16 64~227 |
169 | ||||
170 | Indirect Address 2 | 209 | ||
171 | ||||
172,173 ~ 202,203 | Indirect Address 3 ~ Indirect Address 18 | 210 ~ 225 | ||
204 | Indirect Address 19 | 226 | ||
205 | ||||
206 | Indirect Address 20 | 227 | ||
207 | ||||
208 | Indirect Data 1 | R/W | 0 | uint8 |
209 | Indirect Data 2 | |||
210 ~ 225 | Indirect Data 3 ~ Indirect Data 18 | |||
226 | Indirect Data 19 | |||
227 | Indirect Data 20 |
RA4M1,DA14531
RA4M1,DA14531
]]>