1: 2017-09-03 (日) 22:20:40 takaboo |
2: 2017-09-03 (日) 22:52:04 takaboo |
| }} | | }} |
| | | |
- | ***角度を指令するコード [#u8f17a6a] | + | ***角度を指令 [#u8f17a6a] |
| 紹介した各処理ルーチンを使用し、Goal Position(596~599)へ-180~180度の範囲で10度ずつ角度を増やして指令します。 | | 紹介した各処理ルーチンを使用し、Goal Position(596~599)へ-180~180度の範囲で10度ずつ角度を増やして指令します。 |
| #html{{ | | #html{{ |
| #include <stdbool.h> | | #include <stdbool.h> |
| #include "dx2lib.h" | | #include "dx2lib.h" |
| + | |
| + | #define _MAX_POS_LIMIT 36 // 32bit |
| + | #define _MIN_POS_LIMIT 40 // 32bit |
| + | #define _PRESENT_POS 611 // 32bit |
| + | #define _TORQUE_ENABLE 562 // 8bit |
| + | #define _GOAL_POS 596 // 32bit |
| | | |
| // 変換用係数 | | // 変換用係数 |
| static struct { | | static struct { |
| int32_t max, min; | | int32_t max, min; |
- | } __attribute__ ((gcc_struct, __packed__)) PosLimit = {0,0}; | + | } __attribute__ ((gcc_struct, __packed__)) PosLimit = { 0, 0 }; |
| | | |
| bool GetMaxMinLimit (TDeviceID dev, uint8_t id) { | | bool GetMaxMinLimit (TDeviceID dev, uint8_t id) { |
| if ((PosLimit.max != 0) && (PosLimit.min != 0)) return true; | | if ((PosLimit.max != 0) && (PosLimit.min != 0)) return true; |
- | else return DX2_ReadBlockData (dev, id, 36, (uint8_t *)&PosLimit, sizeof (PosLimit), NULL); | + | else return DX2_ReadBlockData (dev, id, _MAX_POS_LIMIT, (uint8_t *)&PosLimit, sizeof (PosLimit), NULL); |
| } | | } |
| | | |
| | | |
| bool SetTorqueEnable (TDeviceID dev, uint8_t id, bool en) { | | bool SetTorqueEnable (TDeviceID dev, uint8_t id, bool en) { |
- | return DX2_WriteByteData (dev, id, 562, en ? 1 : 0, NULL); | + | return DX2_WriteByteData (dev, id, _TORQUE_ENABLE, en ? 1 : 0, NULL); |
| } | | } |
| | | |
| bool SetGoalAngle (TDeviceID dev, uint8_t id, double angle) { | | bool SetGoalAngle (TDeviceID dev, uint8_t id, double angle) { |
- | int32_t pos = max (min (AngleToPos (dev, id, angle), PosLimit.min), PosLimit.max); | + | int32_t pos = max (min (AngleToPos (dev, id, angle), PosLimit.max), PosLimit.min); |
- | return DX2_WriteLongData (dev, id, 611, pos, NULL); | + | return DX2_WriteLongData (dev, id, _GOAL_POS, pos, NULL); |
| } | | } |
| | | |
| double GetPresentAngle (TDeviceID dev, uint8_t id) { | | double GetPresentAngle (TDeviceID dev, uint8_t id) { |
| int32_t pos; | | int32_t pos; |
- | if (DX2_ReadLongData (dev, id, 611, (uint32_t *)&pos, NULL)) return PosToAngle (dev, id, pos); | + | if (DX2_ReadLongData (dev, id, _PRESENT_POS, (uint32_t *)&pos, NULL)) return PosToAngle (dev, id, pos); |
| else return 0; | | else return 0; |
| } | | } |
| TDeviceID dev; | | TDeviceID dev; |
| if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) { | | if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) { |
- | if (DX2_Ping (dev, 1, NULL)) { | + | printf ("Opened COM port.\n"); |
| + | if (DX2_Ping (dev, 1, NULL)) { // 存在の確認 |
| + | printf ("Found Dynamixel.\n"); |
| SetTorqueEnable (dev, 1, true); // 制御開始 | | SetTorqueEnable (dev, 1, true); // 制御開始 |
- | // -180~180度を10度刻みで | + | int pang, ang; |
- | for (int ang = -180; ang < 180; ang += 10) { | + | pang = GetPresentAngle (dev, 1); // 現在角度を取得 |
| + | // 10度刻みで180度ぐらいまでs |
| + | for (ang = pang; ang < 180; ang += 10) { |
| SetGoalAngle (dev, 1, ang); // 角度指令 | | SetGoalAngle (dev, 1, ang); // 角度指令 |
- | for (int i = 0; i < 10; i++) { | + | for (int i = 0; i < 20; i++) { |
- | printf ("\rGoalPos=%4d, PresentPos=%4.1f", ang, GetPresentAngle (dev, 1)); | + | printf ("\rGoalPos=%4d, PresentPos=%6.1f", ang, GetPresentAngle (dev, 1)); |
- | Sleep (10); | + | Sleep (5); |
| } | | } |
| } | | } |
| + | // 10度刻みで-180度ぐらいまで |
| + | for (;ang > -180; ang -= 10) { |
| + | SetGoalAngle (dev, 1, ang); // 角度指令 |
| + | for (int i = 0; i < 20; i++) { |
| + | printf ("\rGoalPos=%4d, PresentPos=%6.1f", ang, GetPresentAngle (dev, 1)); |
| + | Sleep (5); |
| + | } |
| + | } |
| + | // 10度刻みで最初の位置ぐらいまで |
| + | for (;ang <= pang; ang += 10) { |
| + | SetGoalAngle (dev, 1, ang); // 角度指令 |
| + | for (int i = 0; i < 20; i++) { |
| + | printf ("\rGoalPos=%4d, PresentPos=%6.1f", ang, GetPresentAngle (dev, 1)); |
| + | Sleep (5); |
| + | } |
| + | } |
| + | Sleep (500); |
| SetTorqueEnable (dev, 1, false); // 制御停止 | | SetTorqueEnable (dev, 1, false); // 制御停止 |
| + | } else { |
| + | printf ("Not found Dynamixel.\n"); |
| } | | } |
| + | DX2_ClosePort (dev); |
| + | } else |
| + | printf ("Couldnot open COM port.\n"); |
| + | printf ("\nFin."); |
| + | } |
| + | </pre> |
| + | }} |
| + | |
| + | ***IDの変更 [#f5276f71] |
| + | IDの変更自体は該当のアドレスに新しいIDを書き込むだけですが、変更先のIDを持ったデバイスが無い事を確認した上で変更するプログラムを紹介します。 |
| + | #html{{ |
| + | <pre class="brush: c"> |
| + | #include <stdio.h> |
| + | #include <stdint.h> |
| + | #include <stdbool.h> |
| + | #include "dx2lib.h" |
| + | |
| + | #define _TORQUE_ENABLE 562 // 8bit |
| + | #define _ID 7 // 8bit |
| + | |
| + | bool SetTorqueEnable (TDeviceID dev, uint8_t id, bool en) { |
| + | return DX2_WriteByteData (dev, id, _TORQUE_ENABLE, en ? 1 : 0, NULL); |
| + | } |
| + | |
| + | void main (void) { |
| + | TDeviceID dev; |
| + | int id1, id2; |
| + | if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) { |
| + | printf ("input previous id = "); |
| + | scanf ("%d", &id1); |
| + | printf ("input following id = "); |
| + | scanf ("%d", &id2); |
| + | if (id1 != id2) { |
| + | // 元のIDのデバイスを検索 |
| + | if (DX2_Ping (dev, id1, NULL)) { |
| + | // 変更先のIDのデバイスを検索 |
| + | if (!DX2_Ping (dev, id2, NULL) && !DX2_Ping (dev, id2, NULL)) { |
| + | printf ("Change ID %d->%d ", id1, id2); |
| + | // トルクディスエーブル(必須) |
| + | DX2_WriteByteData (dev, id1, _TORQUE_ENABLE, 0, NULL); |
| + | // 新しいIDを書き込み |
| + | DX2_WriteByteData (dev, id1, _ID, id2, NULL); |
| + | if (DX2_Ping (dev, id2, NULL)) printf ("OK\n"); |
| + | else printf ("NG!\n"); |
| + | } else |
| + | printf ("ID(%d) is in conflict\n", id2); |
| + | } else |
| + | printf ("Cannot find ID(%d)\n", id1); |
| + | } else |
| + | printf ("Unnecessary to change\n"); |
| DX2_ClosePort (dev); | | DX2_ClosePort (dev); |
| } | | } |
| </pre> | | </pre> |
| }} | | }} |
| + | |
| + | ***ボーレートの変更 [#nd7a9226] |