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