**諸条件 [#k0392274] [[DX2LIB]]を用いながら、最低限の制御方法を紹介します。また、Dynamixelのコントロールテーブルは出荷時の状態であるものとし、紹介するプログラム中では対象のIDを1、Baudrateを57600[bps]で固定しています。~ なお、ホストコントローラとしてWindows搭載のPCに[[DXHUB]]をUSBポートを介して接続し、Windows上ではDXHUBがCOM7として認識されているものとしたため、環境に応じて読み替えて下さい。 ***コードのテンプレート [#v6d3a617] COMポートをDX2LIBでオープンするところから始まり、使い終わったらクローズして終わる以下のコードが最低限必要です。 #html{{ <pre class="brush: c"> #include <stdint.h> #include <stdbool.h> #include "dx2lib.h" void main (void) { TDeviceID dev; // ポートオープン if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) { //--------------------- // ここに制御プログラムを追加 //--------------------- // ポートクローズ DX2_ClosePort (dev); } } </pre> }} また、アイテムによって要求するデータ幅が異なりますので、それに応じて読み書きするAPIを使い分ける必要があります。 #html{{ <pre class="brush: c"> uint8_t id; uint16_t addr; //8bit幅 uint8_t d8; DX2_ReadByteData (dev, id, addr, &d8, NULL); // 8bit読み出し DX2_WriteByteData (dev, id, addr, d8, NULL); // 8bit書き込み //16bit幅 uint16_t d16; DX2_ReadWordData (dev, id, addr, &d16, NULL); // 16bit読み出し DX2_WriteWordData (dev, id, addr, d16, NULL); // 16bit書き込み //32bit幅 uint32_t d32; DX2_ReadLongData (dev, id, addr, &d32, NULL); // 32bit読み出し DX2_WriteLongData (dev, id, addr, d32, NULL); // 32bit書き込み //任意サイズ uint8_t da[16]; DX2_ReadBlockData (dev, id, addr, da, sizeof(da), NULL); // 16byte読み出し DX2_WriteBlockData (dev, id, addr, da, sizeof(da), NULL); // 16byte書き込み </pre> }} なお、APIが返すエラー情報はここでは無視しています。 ***サブルーチン [#bd74705b] 必要最低限の動作フローを元しているため、本来行わなくてはならないエラー処理をかなり省略しています。特に返り値のみで判断せざるを得ないサブルーチンは、使用しているAPIでエラーが発生すると想定外の動作を励起する事が想定されます。~ 信頼性の高いシステムを構築する場合は可能な限り厳密なエラー処理を追加し、処理が容易に進捗しないコードを心がけるべきです。 ****トルクイネーブル [#wbcb12a2] 出力軸を動かすには、予めTorque Enable(562)に1を書き込んでおく必要があります。また、制御を停止し出力軸をフリーにするにはTorque Enableに0を書き込みます。~ なお、Torque Enableが0の状態から1を書き込んだ直後、出力軸はPresent PositionをGoal Positionとして位置決め制御を開始しますので、Torque Enableの操作のみで出力軸が実際に回転し始めることはありません。 #html{{ <pre class="brush: c"> // 制御開始 bool SetTorqueEnable (TDeviceID dev, uint8_t id) { return DX2_WriteByteData (dev, id, 562, 1, NULL); } // フリー bool SetTorqueDisable (TDeviceID dev, uint8_t id) { return DX2_WriteByteData (dev, id, 562, 0, NULL); } </pre> }} ****角度と位置の変換 [#o0d33109] モデル毎に扱うGoal Positionの分解能が異なり、角度を元に位置を指令するにしても変換が必要です。~ Max Position Limit(36~39)やMin Position Limit(40~43)が初期状態であれば、それら利用して係数を算出しておき、モデル毎の差異を吸収して角度と位置を相互に変換できます。 #html{{ <pre class="brush: c"> // 変換用係数 static double AngleToPosGain = 0; // 位置上下限リミット値 static int32_t maxpos = 0, minpos = 0; // 位置上下限値取得 bool GetMaxMinLimit (TDeviceID dev, uint8_t id) { if ((maxpos != 0) && (minpos != 0)) return true; else { return DX2_ReadLongData (dev, id, 36, (uint32_t *)&maxpos, NULL) && DX2_ReadLongData (dev, id, 40, (uint32_t *)&minpos, NULL); } } // 角度を位置に変換 int32_t AngleToPos (TDeviceID dev, uint8_t id, double angle) { if ((AngleToPosGain == 0) && GetMaxMinLimit (dev, id)) { AngleToPosGain = (double)maxpos / 180.0; } return angle * AngleToPosGain; } // 位置を角度に変換 double PosToAngle (TDeviceID dev, uint8_t id, int32_t pos) { if ((AngleToPosGain == 0) && GetMaxMinLimit (dev, id)) { AngleToPosGain = (double)maxpos / 180.0; } if (AngleToPosGain == 0) return 0; else return (double)pos / AngleToPosGain; } </pre> }} ****現在の出力軸の角度を取得 [#o8baa7ac] Torque Enableが0であれば、外力で出力軸を回転することができます。その時のPresent Position(611~614)を取得する事で、様々な制御に応用する事ができます。 #html{{ <pre class="brush: c"> // 現在角度を取得 double GetPresentAngle (TDeviceID dev, uint8_t id) { int32_t pos; if (DX2_ReadLongData (dev, id, 611, (uint32_t *)&pos, NULL)) return PosToAngle (dev, id, pos); else return 0; } </pre> }} ****出力軸の角度を指令 [#ea03d19f] Torque Enableが1の状態であれば、Goal Position(596~599)へ書き込んだ値に応じて出力軸が移動します。Goal PositionはMax Position Limit~Min Position Limitの範囲を超えた値を受け取らないため、ここでは上下限値で飽和させた上で指令しています。 #html{{ <pre class="brush: c"> // 角度指令 bool SetGoalAngle (TDeviceID dev, uint8_t id, double angle) { int32_t pos = max (min (AngleToPos (dev, id, angle), minpos), maxpos); return DX2_WriteLongData (dev, id, 611, pos, NULL); } </pre> }} ***角度を指令するコード [#u8f17a6a] 紹介した各処理ルーチンを使用し、Goal Position(596~599)へ-180~180度の範囲で10度ずつ角度を増やして指令します。 #html{{ <pre class="brush: c"> #include <stdio.h> #include <stdint.h> #include <stdbool.h> #include "dx2lib.h" // 変換用係数 static double AngleToPosGain = 0; // 上下限リミット値 static struct { int32_t max, min; } __attribute__ ((gcc_struct, __packed__)) PosLimit = {0,0}; bool GetMaxMinLimit (TDeviceID dev, uint8_t id) { if ((PosLimit.max != 0) && (PosLimit.min != 0)) return true; else return DX2_ReadBlockData (dev, id, 36, (uint8_t *)&PosLimit, sizeof (PosLimit), NULL); } int32_t AngleToPos (TDeviceID dev, uint8_t id, double angle) { if ((AngleToPosGain == 0) && GetMaxMinLimit (dev, id)) { AngleToPosGain = (double)PosLimit.max / 180.0; } return angle * AngleToPosGain; } double PosToAngle (TDeviceID dev, uint8_t id, int32_t pos) { if ((AngleToPosGain == 0) && GetMaxMinLimit (dev, id)) { AngleToPosGain = (double)PosLimit.max / 180.0; } if (AngleToPosGain == 0) return 0; else return (double)pos / AngleToPosGain; } bool SetTorqueEnable (TDeviceID dev, uint8_t id, bool en) { return DX2_WriteByteData (dev, id, 562, en ? 1 : 0, NULL); } bool SetGoalAngle (TDeviceID dev, uint8_t id, double angle) { int32_t pos = max (min (AngleToPos (dev, id, angle), PosLimit.min), PosLimit.max); return DX2_WriteLongData (dev, id, 611, pos, NULL); } double GetPresentAngle (TDeviceID dev, uint8_t id) { int32_t pos; if (DX2_ReadLongData (dev, id, 611, (uint32_t *)&pos, NULL)) return PosToAngle (dev, id, pos); else return 0; } void main (void) { TDeviceID dev; if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) { if (DX2_Ping (dev, 1, NULL)) { SetTorqueEnable (dev, 1, true); // 制御開始 // -180~180度を10度刻みで for (int ang = -180; ang < 180; ang += 10) { SetGoalAngle (dev, 1, ang); // 角度指令 for (int i = 0; i < 10; i++) { printf ("\rGoalPos=%4d, PresentPos=%4.1f", ang, GetPresentAngle (dev, 1)); Sleep (10); } } SetTorqueEnable (dev, 1, false); // 制御停止 } DX2_ClosePort (dev); } } </pre> }}