1: 2017-09-03 (日) 22:20:40 takaboo | |||
---|---|---|---|
Line 1: | Line 1: | ||
+ | **諸条件 [#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> | ||
+ | }} |