Basic operation of PRO using dx2lib

諸条件

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

コードのテンプレート

COMポートをDX2LIBでオープンするところから始まり、使い終わったらクローズして終わる以下のコードが最低限必要です。

#include <stdint.h>
#include <stdbool.h>
#include "dx2lib.h"

void main (void) {
  TDeviceID dev;

  // ポートオープン
  if ((dev = DX2_OpenPort ("\\\\.\\COM7", 57600))) {
    //---------------------
    // ここに制御プログラムを追加
    //---------------------

    // ポートクローズ
    DX2_ClosePort (dev);
  }
}

また、アイテムによって要求するデータ幅が異なりますので、それに応じて読み書きするAPIを使い分ける必要があります。

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書き込み

なお、APIが返すエラー情報はここでは無視しています。

各種サブルーチン

ここでは必要最低限の動作フローを元しているため、本来行わなくてはならないエラー処理をかなり省略しています。特に返り値のみで判断せざるを得ないサブルーチンは、使用しているAPIでエラーが発生すると想定外の動作を励起する事が想定されます。
信頼性の高いシステムを構築する場合は可能な限り厳密なエラー処理を追加し、処理が容易に進捗しないコードを心がけるべきなので、あくまで参考として紹介します。

Dynamixel PROシリーズはモデルによってコントロールテーブル上の各アイテムの扱う値の範囲が異なっていたり、物理値を扱うものやDynamixelの内部でのみ意味のある値があるため、各アイテムを直接扱うとなると互換性のあるプログラムが作りにくい構成になっています。
ここでは主にモデル間の差異を吸収して角度として扱うためのサブルーチンと、動作するにあたって最低限必要なサブルーチンを紹介します。
なお、複数軸を同時に扱う場合はこの限りではありません。

トルクイネーブル

出力軸を動かすには、予めTorque Enable(562)に1を書き込んでおく必要があります。また、制御を停止し出力軸をフリーにするにはTorque Enableに0を書き込みます。
なお、Torque Enableが0の状態から1を書き込んだ直後、内部でPresent PositionをGoal Positionにコピーしてから位置決め制御を開始しますので、Torque Enableの操作のみで出力軸が勝手に回転し始めることはありません。

// 制御開始
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);
}

角度と位置及び回転数の変換

モデル毎に扱うGoal Positionの分解能やGoal Velocityの単位が異なっているため、いずれも物理値を元に指令する場合は変換が必要です。
Goal PositionについてはMax Position Limit(36~39)やMin Position Limit(40~43)が初期状態であれば、それら利用して係数を算出しておき、モデル毎の差異を吸収して角度と位置を相互に変換できます。

// 変換用係数
static double AngleToPosGain = 0;
// 位置上下限リミット値
static int32_t maxvelo = 0, maxpos = 0, minpos = 0;

// 位置上下限値取得
bool GetMaxMinLimit (TDeviceID dev, uint8_t id) {
  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);
  }
}

// 角度を位置に変換
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;
}

現在の出力軸の角度を取得

Torque Enableが0であれば、外力で出力軸を回転することができます。その時のPresent Position(611~614)を取得する事で、様々な制御に応用する事ができます。

// 現在角度を取得
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;
}

出力軸の角度を指令

Operating Mode(11)が1,3でかつTorque Enable(562)が1の状態であれば、Goal Velocity(600~603)へ書き込んだ値に応じて出力軸の回転速度が決まります。Goal Velocityの絶対値がVelocity Limit(32~35)を超えた値を受け取らないため、ここではその値で飽和させた上で指令しています。

// 回転数指令
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);
}

出力軸の回転速度を指令

Operating Mode(11)が3でかつTorque Enable(562)が1の状態であれば、Goal Position(596~599)へ書き込んだ値に応じて出力軸が移動します。Goal PositionはMax Position Limit~Min Position Limitの範囲を超えた値を受け取らないため、ここでは上下限値で飽和させた上で指令しています。

// 角度指令
bool SetGoalVelocity (TDeviceID dev, uint8_t id, double rpm) {
  int32_t velo = max (min (RpmToVelo (dev, id, angle), -maxvelo), maxvelo);
  return DX2_WriteLongData (dev, id, 600, pos, NULL);
}

動作モードの変更

Operating Mode(11)のデフォルト値はPosition Control Modeですが、モードが違えば操作も異なりますし、思い通りの動作が行われない事も起こりえます。
なお、動作モードの変更はTorque Enable(562)が0の状態でのみ可能です。

// モード設定
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;
}

角度を指令

紹介した各処理ルーチンを使用し、現在位置を起点に最大動作範囲を往復回転させるプログラムです。

#include <stdio.h>
#include <stdint.h>
#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

// 変換用係数
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.");
}

IDの変更

IDの変更は該当のアドレスに新しいIDを書き込む事で行いますが、複数のデバイスが接続された環境での変更はIDの競合が発生する等のリスクがあります。
手動で既存のIDを調べながら変更するのは少々手間なので、変更先のIDを持ったデバイスが無い事を確認して更新するプログラムを紹介します。

#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");
}

ボーレートの変更