Renesas Community
Renesas Community
  • User
    Join or sign in
  • Site
  • Search Community
  • User
  • Renesas Engineering Community
  • FAQ
  • HELP
  • More
  • Cancel
がじぇっとるねさすコミュニティ
がじぇっとるねさすコミュニティ
GR-ROSE ROBOTIS Dymamixelシリアルサーボの制御
  • Forums
  • Files
  • がじぇっとるねさす ゆーざー会 - Wiki
  • Tags
  • More
  • Cancel
  • New

 

 GR-SAKURA

 GR-KURUMI

 GR-COTTON

 GR-CITRUS

 GR-PEACH

 GR-KAEDE

 GR-ADZUKI

 GR-LYCHEE

 GR-ROSE

 GR-MANGO(*)

 SNShield

 Web Compiler

 IDE for GR

 TOPPERS関連

 女子美コラボ

 その他

 ※プロデューサミーティング中

 作り方使い方資料

 イベント関連

 作品記事

 体験記事

 その他

 

 ライブラリ

 ツール

 その他・過去ファイル

  • Replies 6 replies
  • Subscribers 438 subscribers
  • Views 10545 views
  • Users 0 members are here
Options
  • Share
  • More
  • Cancel
Related Tags
  • 3D
  • analog
  • B3M
  • DDS-XRCE
  • endif
  • esp8266
  • FIT
  • GR-ROSE
  • ICS
  • include
  • MG996R
  • MIC3
  • microSD
  • MPL3115A2
  • OLED
  • pin
  • PMOD
  • ROS
  • ROS2
  • rosserial
  • RS-485
  • RX65N
  • SDHI
  • serial
  • SmartConfigurator
Related

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

西村備山
西村備山 over 4 years ago

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);
  }
}
  • Reply
  • Cancel
  • Cancel
  • 西村備山
    西村備山 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
    Okamiya Yuuki over 4 years ago in reply to 西村備山
    早速使っていただいてありがとうございます!
    • Cancel
    • Up 0 Down
    • Reply
    • Cancel
  • Okamiya Yuuki
    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() {
    }
    • Cancel
    • Up 0 Down
    • Reply
    • Cancel
  • JiGoRo
    JiGoRo over 4 years ago
    貴重なサンプルコードありがとうございます。

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

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

    この設定が正しいのでしょうか? それともV2のGoal Position のアドレス 116 が正しいのでしょうか?
    • Cancel
    • Up 0 Down
    • Reply
    • Cancel
  • JiGoRo
    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
サイト使用条件
プライバシーポリシー
お問い合わせ
© 2010-2023 Renesas Electronics Corporation. All rights reserved.