Basic operation of PRO using dx2lib のバックアップソース(No.2) :: Besttechnology

• 差分^[1] を表示
• 現在との差分^[2] を表示
• Basic operation of PRO using dx2lib^[3] へ行く。

**諸条件 [#k0392274]
[[DX2LIB]]を用いながら、最低限の制御方法を紹介します。また、Dynamixelのコントロールテーブルは出荷時の状態であるものとし、紹介するプログラム中では対象のIDを1、Baudrateを57600[bps]で固定しています。~
なお、ホストコントローラとしてWindows搭載のPCに[[DXHUB]]をUSBポートを介して接続し、Windows上ではDXHUBがCOM7として認識されているものとしたため、環境に応じて読み替えて下さい。

***コードのテンプレート [#v6d3a617]
COMポートをDX2LIBでオープンするところから始まり、使い終わったらクローズして終わる以下のコードが最低限必要です。
#html{{
<pre class="brush: c">
#include &lt;stdint.h&gt;
#include &lt;stdbool.h&gt;
#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 &lt;stdio.h&gt;
#include &lt;stdint.h&gt;
#include &lt;stdbool.h&gt;
#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 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, _MAX_POS_LIMIT, (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, _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.max), PosLimit.min);
  return DX2_WriteLongData (dev, id, _GOAL_POS, pos, NULL);
}

double GetPresentAngle (TDeviceID dev, uint8_t id) {
  int32_t pos;
  if (DX2_ReadLongData (dev, id, _PRESENT_POS, (uint32_t *)&pos, NULL)) return PosToAngle (dev, id, pos);
  else return 0;
}

void main (void) {
  TDeviceID dev;
  if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) {
    printf ("Opened COM port.\n");
    if (DX2_Ping (dev, 1, NULL)) {      // 存在の確認
      printf ("Found Dynamixel.\n");
      SetTorqueEnable (dev, 1, true);   // 制御開始
      int pang, ang;
      pang = GetPresentAngle (dev, 1);  // 現在角度を取得
      // 10度刻みで180度ぐらいまでs
      for (ang = pang; 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度刻みで-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を持ったデバイスが無い事を確認した上で変更するプログラムを紹介します。
#html{{
<pre class="brush: c">
#include &lt;stdio.h&gt;
#include &lt;stdint.h&gt;
#include &lt;stdbool.h&gt;
#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);
  }
}
</pre>
}}

***ボーレートの変更 [#nd7a9226]

• Basic operation of PRO using dx2lib のバックアップ一覧^[6]
• Basic operation of PRO using dx2lib のバックアップソース(No. All)
  • 1: 2017-09-03 (日) 22:20:40^[7] takaboo^[8]
  • 2: 2017-09-03 (日) 22:52:04 takaboo^[8]
  • 現: 2017-09-04 (月) 18:01:25^[9] takaboo^[8]