ページへ戻る

+ Links

 印刷 

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

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

  Next »
**諸条件 [#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"

// 変換用係数
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>
}}

  Next »