Besttechnology - [Basic operation of PRO using dx2lib のバックアップの現在との差分(No.1)]

差分を表示
ソースを表示
Basic operation of PRO using dx2lib へ行く。

--- 1: 2017-09-03 (日) 22:20:40 takaboo
+++ 現: 2017-09-04 (月) 18:01:25 takaboo
@@ Line 55: / Line 55: @@
 なお、APIが返すエラー情報はここでは無視しています。
-***サブルーチン [#bd74705b]
+***各種サブルーチン [#bd74705b]
-必要最低限の動作フローを元しているため、本来行わなくてはならないエラー処理をかなり省略しています。特に返り値のみで判断せざるを得ないサブルーチンは、使用しているAPIでエラーが発生すると想定外の動作を励起する事が想定されます。~
+ここでは必要最低限の動作フローを元しているため、本来行わなくてはならないエラー処理をかなり省略しています。特に返り値のみで判断せざるを得ないサブルーチンは、使用しているAPIでエラーが発生すると想定外の動作を励起する事が想定されます。~
-信頼性の高いシステムを構築する場合は可能な限り厳密なエラー処理を追加し、処理が容易に進捗しないコードを心がけるべきです。
+信頼性の高いシステムを構築する場合は可能な限り厳密なエラー処理を追加し、処理が容易に進捗しないコードを心がけるべきなので、あくまで参考として紹介します。
+Dynamixel PROシリーズはモデルによってコントロールテーブル上の各アイテムの扱う値の範囲が異なっていたり、物理値を扱うものやDynamixelの内部でのみ意味のある値があるため、各アイテムを直接扱うとなると互換性のあるプログラムが作りにくい構成になっています。~
+ここでは主にモデル間の差異を吸収して角度として扱うためのサブルーチンと、動作するにあたって最低限必要なサブルーチンを紹介します。~
+なお、複数軸を同時に扱う場合はこの限りではありません。
 ****トルクイネーブル [#wbcb12a2]
 出力軸を動かすには、予めTorque Enable(562)に1を書き込んでおく必要があります。また、制御を停止し出力軸をフリーにするにはTorque Enableに0を書き込みます。~
-なお、Torque Enableが0の状態から1を書き込んだ直後、出力軸はPresent PositionをGoal Positionとして位置決め制御を開始しますので、Torque Enableの操作のみで出力軸が実際に回転し始めることはありません。
+なお、Torque Enableが0の状態から1を書き込んだ直後、内部でPresent PositionをGoal Positionにコピーしてから位置決め制御を開始しますので、Torque Enableの操作のみで出力軸が勝手に回転し始めることはありません。
 #html{{
 <pre class="brush: c">
@@ Line 76: / Line 80: @@
 }}
-****角度と位置の変換 [#o0d33109]
+****角度と位置及び回転数の変換 [#o0d33109]
-モデル毎に扱うGoal Positionの分解能が異なり、角度を元に位置を指令するにしても変換が必要です。~
+モデル毎に扱うGoal Positionの分解能やGoal Velocityの単位が異なっているため、いずれも物理値を元に指令する場合は変換が必要です。~
-Max Position Limit(36~39)やMin Position Limit(40~43)が初期状態であれば、それら利用して係数を算出しておき、モデル毎の差異を吸収して角度と位置を相互に変換できます。
+Goal PositionについてはMax Position Limit(36~39)やMin Position Limit(40~43)が初期状態であれば、それら利用して係数を算出しておき、モデル毎の差異を吸収して角度と位置を相互に変換できます。
 #html{{
 <pre class="brush: c">
@@ Line 84: / Line 88: @@
 static double AngleToPosGain = 0;
 // 位置上下限リミット値
-static int32_t maxpos = 0, minpos = 0;
+static int32_t maxvelo = 0, maxpos = 0, minpos = 0;
 // 位置上下限値取得
 bool GetMaxMinLimit (TDeviceID dev, uint8_t id) {
-  if ((maxpos != 0) && (minpos != 0)) return true;
+  if ((maxvelo != 0) && (maxpos != 0) && (minpos != 0)) return true;
   else {
     return
+      DX2_ReadLongData (dev, id, 32, (uint32_t *)&maxvelo, NULL) &&
       DX2_ReadLongData (dev, id, 36, (uint32_t *)&maxpos, NULL) &&
       DX2_ReadLongData (dev, id, 40, (uint32_t *)&minpos, NULL);
@@ Line 131: / Line 136: @@
 ****出力軸の角度を指令 [#ea03d19f]
-Torque Enableが1の状態であれば、Goal Position(596~599)へ書き込んだ値に応じて出力軸が移動します。Goal PositionはMax Position Limit~Min Position Limitの範囲を超えた値を受け取らないため、ここでは上下限値で飽和させた上で指令しています。
+Operating Mode(11)が1,3でかつTorque Enable(562)が1の状態であれば、Goal Velocity(600~603)へ書き込んだ値に応じて出力軸の回転速度が決まります。Goal Velocityの絶対値がVelocity Limit(32~35)を超えた値を受け取らないため、ここではその値で飽和させた上で指令しています。
+#html{{
+<pre class="brush: c">
+// 回転数指令
+bool SetGoalVelocity (TDeviceID dev, uint8_t id, double rpm) {
+  int32_t velo = max (min (RpmToVelo (dev, id, rpm), -maxvelo), maxvelo);
+  return DX2_WriteLongData (dev, id, 600, velo, NULL);
+}
+</pre>
+}}
+****出力軸の回転速度を指令 [#ea03d19f]
+Operating Mode(11)が3でかつTorque Enable(562)が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) {
+bool SetGoalVelocity (TDeviceID dev, uint8_t id, double rpm) {
-  int32_t pos = max (min (AngleToPos (dev, id, angle), minpos), maxpos);
+  int32_t velo = max (min (RpmToVelo (dev, id, angle), -maxvelo), maxvelo);
-  return DX2_WriteLongData (dev, id, 611, pos, NULL);
+  return DX2_WriteLongData (dev, id, 600, pos, NULL);
 }
 </pre>
 }}
-***角度を指令するコード [#u8f17a6a]
+****動作モードの変更 [#vfdea87f]
-紹介した各処理ルーチンを使用し、Goal Position(596~599)へ-180～180度の範囲で10度ずつ角度を増やして指令します。
+Operating Mode(11)のデフォルト値はPosition Control Modeですが、モードが違えば操作も異なりますし、思い通りの動作が行われない事も起こりえます。~
+なお、動作モードの変更はTorque Enable(562)が0の状態でのみ可能です。
+#html{{
+<pre class="brush: c">
+// モード設定
+bool SetOperatingMode (TDeviceID dev, uint8_t id, uint8_t mode) {
+  if ((mode >=0) && (mode <= 4)) {
+    if (DX2_WriteByteData (dev, id, 562, 0, NULL))
+      return DX2_WriteByteData (dev, id, 11, mode, NULL);
+  }
+  return false;
+}
+</pre>
+}}
+***角度を指令 [#u8f17a6a]
+紹介した各処理ルーチンを使用し、現在位置を起点に最大動作範囲を往復回転させるプログラムです。
 #html{{
 <pre class="brush: c">
@@ Line 150: / Line 183: @@
 #include <stdbool.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 195: @@
 static struct {
   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) {
   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 218: @@
 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) {
-  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) {
   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;
 }
@@ Line 196: / Line 235: @@
   TDeviceID dev;
   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);   // 制御開始
-      // -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);     // 角度指令
-        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);  // 制御停止
+    } else {
+      printf ("Not found Dynamixel.\n");
     }
     DX2_ClosePort (dev);
-  }
+  } else
+    printf ("Couldnot open COM port.\n");
+  printf ("\nFin.");
 }
 </pre>
 }}
+***IDの変更 [#f5276f71]
+IDの変更は該当のアドレスに新しいIDを書き込む事で行いますが、複数のデバイスが接続された環境での変更は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
+void main (void) {
+  TDeviceID dev;
+  int id1, id2;
+  printf ("input current ID number = ");
+  scanf ("%d", &id1);
+  printf ("input updated ID number = ");
+  scanf ("%d", &id2);
+  if (id1 != id2) {
+    if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) {
+      printf ("Opened COM port.\n");
+      // 元の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 ("Couldnot find ID(%d)\n", id1);
+      DX2_ClosePort (dev);
+    } else
+      printf ("Couldnot open COM port.\n");
+  } else
+    printf ("Unnecessary to change\n");
+}
+</pre>
+}}
+***ボーレートの変更 [#nd7a9226]

Basic operation of PRO using dx2lib のバックアップ一覧
Basic operation of PRO using dx2lib のバックアップの現在との差分(No. All)
- 1: 2017-09-03 (日) 22:20:40 takaboo
- 2: 2017-09-03 (日) 22:52:04 takaboo

メニュー

トップニュースお知らせ新商品情報更新情報マニュアル類技術情報チュートリアル FAQ お問合せ技術的なお問合せその他お問合せお見積りブログ会社案内

ショッピング

ご利用案内検索カテゴリ一覧ロボットキット Dynamixelシリーズマイコンボード相撲ロボットセンサ通信機器お客様ページ