サンプルプログラムは以下のURLにZIP形式の圧縮ファイルで設置。適宜アーカイバにて解凍して使用の事。
サンプルソースをGCC Developer Liteで開き、ツールメニューのコンパイルオプションをクリックしコンパイラオプションダイアログボックス上端にある設定リストから「FREEDOM III library for FDIII-HC (Bootloader)」を選択してOKボタンを押したら環境の設定は完了。
GCC Developer Liteのコンパイルメニューから「ビルド」をクリックするとコンパイルが開始され、成功すればSIMPLE TERMが開く。FDIII-HCのブートローダがコマンドモードになっていれば、SIMPLE TERMの転送メニューから「スクリプト実行」をクリックしコンパイル済みのファイルを転送する。転送が完了したらブートローダの「g」コマンドで実行。
なお、ほとんどのサンプルがコンソールとしてUSBを使用するため、動作状況はSIMPLE TERMのターミナルウィンドウで確認できる。また、ほとんどのサンプルプログラムは、実行中にターミナルウィンドウ上でESCキーを押すか、FDIII-HCの場合はPB1を押すと実行を終了しブートローダに戻る。
ほぼ全てのサンプルプログラムに共通なシリアルコンソール部分は、shareフォルダのus.cにまとめた。USBやBluetoothといったコンソールの違いがあっても、1行修正すればどの環境にも対応する。
USBをコンソールとして使うなら、
#define __UDP__ #include "share\us.c"
Bluetoothをコンソールとして使うなら、
#define __DBGU2__ #include "share\us.c"
RS232C(SAM7S I/Oのみ)をコンソールとして使うなら、
#define __US1__ #include "share\us.c"
といった具合にサンプルプログラム中の#defineの部分を任意に変更してコンパイルし直せば対応は完了する。
あくまでサンプルプログラムの便宜上分けただけなので、全てのポートを使ったアプリケーションを作成する場合は無視して結構。
ライブラリを使用するにあたって必要最低限の手順を示す。次の2行は必須。
enableIRQ (); // ARMの割り込み許可 DX_Init (1000000, 7.4, NULL, NULL); // DXLIBの初期化
割り込みを許可した後、DX_InitでDynamixelとの通信速度[bps]、電源電圧低下アラーム閾値[V]、DX_PrintBuffer APIで送信対象となるコンソール、200Hz周期で起動される外部関数、を指定すれば初期化は完了となる。後述の2つは使わない限りNULLを指定しておけば大概問題は無い。
Dynamixelに装備されるモニタLEDを明滅させる。特にモータが動くといった訳ではく危険が少ないので、ひとまずDynamixelとの通信のベースとして参照の事。
int j, l2; uint8_t param[128]; static uint8_t tbuf[150], rbuf[150]; uint16_t err;
param[0] = ADDRESS_LED; param[1] = f & 1; j = DX_TxPacket(tbuf, id, INST_WRITE, param, 2, &err); DX_PrintBuffer(" <-",tbuf,j); j = DX_RxPacket(rbuf, sizeof(rbuf), &l2, _DX_READ_RESPONSE_TIME, &err); if (id != BROADCASTING_ID) DX_PrintBuffer(" ->",rbuf,j);
ちょっと面倒だが、インストラクションパケットの送信とステータスパケットの受信を行うには最低限これだけの記述が必要。また、DX_TxPacket及びDX_RxPacketの内部処理は非同期に行われるため、バッファはstaticな物を与える事。
もちろんこんなまどろっこしい記述をやめて、以下の様にDX_WriteByteDataでさらっと済ませても良い。
uint16_t err; DX_WriteByteData (id, ADDRESS_LED, f, 100, &err);
Dynamixelのモータを実際に動かして任意の位置にホーンを移動させる。一応ID:1~3の3台のDynamixelが存在している事を想定しているが、1台でも動作確認は可能。
送信の仕方として3種類用意されており、1軸毎に処理(個別IDへインストラクションパケット送信)、軸数関係なく全て同時に処理(BROADCAST IDでインストラクションパケット送信)、軸数を意識して全て同時に処理(同期インストラクションパケットを使って送信)を行っている。
手始めは1軸ずつちまちま送信してみて、軸数を増やして1軸毎にインストラクションパケットを処理した際にオーバヘッドが気になるようになってきたら同期インストラクションパケットで一気に指令するといった具合に移行すれば良いかと。
ここでも面倒だがインストラクションパケットを直に生成している部分が多々あるが、DX_ReadWordDataやDX_SetPositionに置き換えれば見通しが良くなる。
Dynamixelのホーンの位置を取得しモニタする。
ここでも更に面倒だがインストラクションパケットを直に生成しているが、DX_ReadWordDataを使えばこれまたすっきりする。
short GetPos (uint8_t id) { uint16_t result; uint16_t err; return DX_ReadWordData (id, ADDRESS_PRESENT_POSITION, &result, 100, &err) ? result : -1; }
スムーズに位置を遷移させるにはA地点からB地点の間に移動する速度を与えて指令すれば解決するが、移動時間はその速度の分解能に依存してしまう。
ここではA地点からB地点へ移動する時間を指定して動作させているが、結局小刻みに位置を変化させて指令しているに過ぎない。
なお3軸まで対応させているが、便宜上ID=1だけを対象に動作するようにしている。
出荷時にIDは全て1のDynamixelだが、複数台つないで使うときは全て異なる数値にする必要がある。通常コンフィギュレータを使用してPCから設定するものとしているが、IDと数個のパラメータだけだが変更できるプログラムを作ってみた。
動作チェックも可能。
id:1baud:34(UNKNOWN) >? ----Command List---- [p/P] Ping [s/S] Scan [i] Change host id [I] Change target id [b] Change host baudrate [B] Change target baudrate [R] Change target return delay time [t/T] Test run
こんな感じでいくつかの機能を提供している。
使い方は、コントローラが対象としたいidとボーレートを「i」と「b」で設定し、「p」で通信できるかを確認。idもボーレートも分からなければ「s」でスキャンをかけ、見つかったidとボーレートを設定すれば良い。通信出来る事が確認できたらidやボーレートを「I」と「B」で変更する。といった具合。
FDIII-HCには既に備わっているパケットスルーモードだが、USB限定でユーザプログラムとしてソースに起こしてみた。全くDynamixelライブラリを使用していないので参考にならないかもしれない。
FDIII-HCのブートローダにおいてUSBを介したパケットスルーモードが正常に動作しない場合はこのプログラムを試してほしい。
先のthrough_usb_to_485.c同様にブートローダに予め備わっているBluetoothによるパケットスルーをコーディングしたもの。
Bluetoothを介してボーレートを変更する事が出来ないので、とりあえず1Mbpsで初期化している。
ここまで書いておいて何だが、大抵の場合はdx.hに宣言されているDynamixel専用のAPIを使えばいちいちインストラクションパケットを意識する必要はない。位置指令であれば、
bool DX_SetPosition (uint8_t id, int p);
を使えば任意のIDへ任意の位置を指定できるし、
bool DX_ReadWordData (uint8_t id, uint8_t adr, uint16_t *result, uint32_t timeout, uint16_t *errcode);
を使えば任意のIDの任意のアドレスに保存されている16bit幅までの値を簡単にかつ少ないコード量で取り出す事ができる。
詳細はライブラリのソースで確認してほしい。