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

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

@@ 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>
+}}

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
- 現: 2017-09-04 (月) 18:01:25 takaboo

メニュー

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

ショッピング

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