1: 2017-09-03 (日) 22:20:40 takaboo ソース 2: 2017-09-03 (日) 22:52:04 takaboo ソース
Line 142: Line 142:
}} }}
-***角度を指令するコード [#u8f17a6a]+***角度を指令 [#u8f17a6a]
紹介した各処理ルーチンを使用し、Goal Position(596~599)へ-180～180度の範囲で10度ずつ角度を増やして指令します。 紹介した各処理ルーチンを使用し、Goal Position(596~599)へ-180～180度の範囲で10度ずつ角度を増やして指令します。
#html{{ #html{{
Line 150: Line 150:
#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
// 変換用係数 // 変換用係数
Line 156: Line 162:
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);
} }
Line 179: Line 185:
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;
} }
Line 196: Line 202:
  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);
  }   }
Line 213: Line 292:
</pre> </pre>
}} }}
 +
 +***ボーレートの変更 [#nd7a9226]
トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom