ROBOTIS Dynamixelシリアルサーボの制御

Dynamixelについて

ROBOTIS Dynamixelはロボット用シリアルサーボのシリーズ。

Dynamixel XL-320の仕様

f:id:licheng:20181119163436j:plain
XL-320は、小型二足歩行ロボット DARWIN-MINI に採用されいている。

  • 電圧: 6 ~ 8.4V (推奨: 7.4V)
  • トルク: 3.82kg-cm (0.39Nm)
  • 重量:16.7g / サイズ:24mm x 36mm x 27mm
  • 通信プロトコル: Protocol 2.0 (V2)
  • ボーレート: 7343 bps ~ 1 Mbps (デフォルト: 1Mbps)
  • 市販のシリアルサーボとしてはかなり小さい。実勢価格3千円前後。
  • XL-320 (ROBOTIS 英文資料)

GR-ROSEでの制御

ルネサスRX-65N搭載のGR-ROSEボード(ベータ版)でXL-320を制御したようす。
GR-ROSEは半二重シリアルサーボをSerial1, Serial2に直接挿せる。
プログラムはArduinoベースの環境(IDE for GR)で作成。

GR-ROSE(ベータ版)でDynamixelシリアルサーボ

コード

#include<stdint.h>

// CRC-16-IBM
uint16_t CRC_calc(uint8_t *data, int length)
{
  uint16_t crc16;
  int i,j;
  
  crc16 = 0x0000; // 初期値=0
  for(i=0;i<length;i++){
    crc16 ^= ( ((uint16_t)data[i]) << 8);
    for(j=0;j<8;j++){
      if(crc16 & 0x8000){
        crc16 = (crc16 << 1) ^ 0x8005; // 生成多項式
      }else{
        crc16 <<= 1;
      }
    }
  }
  return crc16;
}

bool sendpos(uint8_t id, uint16_t pos) {
  // インストラクションパケット
  uint8_t tx_buf[] = { // 多バイトの数値はリトルエンディアン
    0xFF, 0xFF, 0xFD, 0x00, // Header(固定値)
    id,         // ID
    0x09, 0x00, // Length
    0x03,       // Instruction (0x03 = WRITE)
    30, 0,      // Address (30 = GoalPosition)
    (uint8_t)(pos & 0xFF), (uint8_t)(pos >> 8), 0x00, 0x00, // Data
    0, 0        // CRC (下で計算)
  };
  // CRC計算
  uint16_t crc = CRC_calc(tx_buf, 14);
  tx_buf[14] = (uint8_t)(crc & 0xFF);
  tx_buf[15] = (uint8_t)(crc >> 8);
  // 送信
  while (Serial1.available ()) Serial1.read (); // バッファクリア
  Serial1.write (tx_buf, 16);
  
  // ステータスパケット受信
  uint8_t rx_buf[16+11];
  if (Serial1.readBytes (rx_buf, 16+11) == 16+11){
    if(rx_buf[16+4] != id){ // IDの一致をチェック
     Serial.println("ID not matched!");
     return false;
    }
    if(rx_buf[16+8] != 0){ // エラーをチェック
     Serial.print("Status Error = "); Serial.println(rx_buf[16+8]);
     return false;
    }
    Serial.println("OK!");
    return true;
  }else{
    Serial.println("Bad response!");
    return false;
  }
}

void setup() {
  Serial1.begin (1000000); // 1Mbps
  Serial1.direction(HALFDUPLEX); // GR-ROSEではこれが必要
  Serial1.setTimeout (50);
  
  Serial.begin(115200);
}
 
void loop() {
  for(int id = 1; id <= 3; id++){
    sendpos (id, 0);
    delay (1000);
    sendpos (id, 1023);
    delay (1000);
  }
}