2014年11月23日日曜日

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); // 動作待
}