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

GR-ROSE作品発表会の賞品のDarwin Mini届きました。
ありがとうございました。

組み立てる前にDynamixelシリアルサーボXL-320を
GR-ROSEで制御してみました。

Dynamixelの通信プロトコルはV1とV2で別物なので要注意です。
Darwin MiniのXL-320はV2で、
ボーレートは1Mbpsがデフォでした。

#include

// 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);
  Serial1.direction(HALFDUPLEX);
  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);
  }
}

西村備山 over 4 years ago

あ、貼り付けたコードの#include が化けてしまいました。
stdint.hです。
Cancel
Up 0 Down

Reply

Cancel
西村備山 over 4 years ago in reply to 西村備山

ブログにかんたんにまとめました。

DymamixelじゃなくてDynamixelですね…

https://lipoyang.hatenablog.com/entry/2018/11/19/083837
Cancel
Up 0 Down

Reply

Cancel
Okamiya Yuuki over 4 years ago in reply to 西村備山

早速使っていただいてありがとうございます！
Cancel
Up 0 Down

Reply

Cancel

Okamiya Yuuki over 4 years ago in reply to Okamiya Yuuki

ROSEのリリースに向けて私もようやくROBOTIS MINIを製作しました。制作中にIDの違うサーボをくっつけてしまいまして、壊すのも面倒なので、IDの変更をROSEを使って行いました。

下記のライブラリを使いました。dxl_pro.cppのupdate_crc()が一部のROSEライブラリと被っていたため、適当に名前を変更して試し、問題なく動作し、無事にID変更もできました。

https://github.com/hackerspace-adelaide/XL320

ちなみに、IDを変更すると9600bpsになってしまうようで、変更後は1Mbpsに戻す必要あります。以下実行したサンプルスケッチを一部変更したもので、#define CHANGE_IDを切り替えてビルド→実行を2回行います。

// ========================================
// Dynamixel XL-320 Arduino library example
// ========================================

// Read more:
// github.com/.../XL320
#include "Arduino.h"
#include "XL320.h"

// Name your robot!
XL320 robot;
// Note: when setting the ServoID, the servos default down to 9600 baud, so after you set the servoID you'll need to set the baud rate back up to 1Mbps(default).
#define CHANGE_ID
void setup() {

 // Talking standard serial, so connect servo data line to Digital TX 1
 // Set the default servo baud rate which is 1000000 (1Mbps) if it's a brand new servo
#ifdef CHANGE_ID
 Serial1.begin(1000000);
#else
 Serial1.begin(9600);
#endif
 Serial1.direction(HALFDUPLEX);
 Serial1.setTimeout(50);

 pinMode(PIN_LED0, OUTPUT);
 digitalWrite(PIN_LED0, LOW);
 pinMode(PIN_LED1, OUTPUT);
 digitalWrite(PIN_LED1, LOW);

 // Initialise your robot
 robot.begin(Serial1); // Hand in the serial object you're using
 delay(100);

 // Current servoID
 int servoID = 254;

 // NOTE: comment out either the XL_BAUD_RATE or XL_ID, only send one at a time

 // ===================================
 // Set the serial connection baud rate
 // ===================================

 // writePacket(1, XL_BAUD_RATE, x) sets the baud rate:
 // 0: 9600, 1:57600, 2:115200, 3:1Mbps
#ifndef CHANGE_ID
 robot.sendPacket(servoID, XL_BAUD_RATE, 3);
#endif

 // ================
 // Set the servo ID
 // ================

 // writePacket(1, XL_ID, x) sets the baud rate:
 // ID can be between 1 and 253 (but not 200)
#ifdef CHANGE_ID
 robot.sendPacket(servoID, XL_ID, 2);
#endif
 digitalWrite(PIN_LED1, HIGH);
}

void loop() {
}

JiGoRo over 4 years ago

貴重なサンプルコードありがとうございます。

Robotis の Dynamixel の V2 プロトコルと書かれてますね。

Goal Position のアドレスが 30 と、V1プロトコルのアドレスになってます。　一方で、パケットのフォーマットはV2。

この設定が正しいのでしょうか？　それともV2のGoal Position のアドレス 116 が正しいのでしょうか？
Cancel
Up 0 Down

Reply

Cancel
JiGoRo over 4 years ago in reply to JiGoRo

自己返信です。

Dynamixel のプロトコル、V1, V2 で設定できるんですね。　ただし、フォーマットはV2のままでレジスタアドレスがV1になる、ということなのでしょうかね。手持ちサーボでやってみましたが、V1モードにしたとき、V1のフォーマットではダメで角度指示などのアドレスだけが変わってました。　なんでこんな仕様になってるんでしょう。。。

ということで、ここにあげていただいているサンプルは、V2をサポートするXL340でV1指定としての制御モードになってるようですね。

正しいでしょうか　＞　有識者の方
Cancel
Up 0 Down

Reply

Cancel