Dynamixel RX-28サーボをArduinoで制御

RS-485経由で制御する Dynamixel RX-28サーボ を、Arduinoで制御してみました。OBがパンチル投影用に2軸でミラー駆動していたもので、その時はPCからUSB経由で制御していました。今回は、SparkFunのRS-485 BreakOutを経由して、Arduinoのシリアル出力（57600ボー）で制御してみました。サーボIDを0（工場出荷時が1なので、0に変更する必要があります）と1とかに切り分けると、RX-28はディジーチェーン接続できます。

接続図（Breakout A端子→RX28 D+、B端子→D-）

連結サーボのベースはカマボコ板

#define BAUDRATE 57600　// ディフォルトボーレート[bps]

#define ADDRESS_GOAL_POSITION 30 // コントロールテーブルでの目標座標のアドレス

#define RX28_HAND 0 // サーボAのモータID(0〜253)

#define RX28_BASE 1 // サーボBのモータID(0〜253)

#define RS485RTS 2 // RS485の送受切替信号。RTSをHIGH固定接続なら不要

//----------------------------------------------

// モータID毎に各々位置指令

// int mID;  移動させるモータID(0x00～0xFD:254台)

// int mPos; 移動目的位置(0-1023)

//

// インストラクションパケット構造

// $FF $FF [ID] [LEN] [INST] [PARAM1]....[PARAMn] [SUM]

// [LEN]:Parameter数(N)+2

// [SUM]:ID+LENGTH+INSTRUCTION+PARAM1+..+PARAMmの下位8ビット分のビット反転

//----------------------------------------------

void RX28_move(int mID, int mPos) {

int sum; // チェックサム

unsigned char buf[10]; // 通信用バッファ

buf[0] = 0xFF; // 固定

buf[1] = 0xFF; // 固定

buf[2] = (unsigned char)mID;

sum = mID; // チェックサム計算開始

buf[3] = (unsigned char)(3 + 2); // データ長 (ID+PARAMs+SUM)

sum += buf[3];

buf[4] = INST_WRITE; // インストラクション

sum += buf[4];

buf[5] = (unsigned char)ADDRESS_GOAL_POSITION; // パラメータ

sum += buf[5];

buf[6] = (unsigned char)(mPos & 0xFF);

sum += buf[6];

buf[7] = (unsigned char)(mPos >> 8);

sum += buf[7];

buf[8] = (unsigned char)(~(sum) & 0xFF); // チェックサム

for(int i = 0; i < 9 ; i++){

 Serial.write(buf[i]);

 delay(1);

}

}

void setup(){

  Serial.begin(BAUDRATE); // シリアルポートを57600 bps[ビット/秒]で初期化 

  pinMode(RS485RTS, OUTPUT); // RS485の送受切替信号を設定

  digitalWrite(2, HIGH); // RS485の送受切替信号にHIGHを出力し、送信モードに設定

  delay(100);

  RX28_move(RX28_BASE, 512); // ベースサーボを 512(中央)に

  RX28_move(RX28_HAND, 512); // ハンドサーボを 512(中央)に

  delay(1000);

}

void loop() {

  RX28_move(RX28_BASE, 712); // ベースサーボを200*(300/1024)度位置へ回転

  RX28_move(RX28_HAND, 400); // ハンドサーボを112*(300/1024)度位置へ回転

  delay(1000); // 動作待

}