DynamixelのXLサーボを動かす

Amazonでは見つかるまい

やること

デフォルト状態のDynamixelのサーボをちょっとだけ動かします。
Dynamixelのサーボのコマンドの基礎に触れ、開発にとっかかれるようにします。

DYNAMIXEL XL330

XL330はお手頃ですがプロトコルも標準の2.0で性能もバッチリですので、最初の１個として最適だと思います。
ほどよい性能のXL330-M288-Tやスピードがとても速いXL330-M077-Tなどがあるようです。
今回のサンプルコードはどちらでも動きました。

作業環境

• Arduino IDE
• ESP32DevkitC
• 半二重回路(Meridian LITEなど)
• DYNAMIXEL XL330

参考

半二重回路とは？

信号線１本で送信、受信を行う場合によく使われる回路です。
半二重回路にマイコンのENピンからのHIGH/LOWで送信、受信を切り替える仕組みです。

3ステートバッファ2つとNot回路を組み合わせて作ることができます。
http://robooptions.blog.fc2.com/blog-entry-10.html
（参考サイト）

また、秋月で売っているRS485ドライバを使えばICひとつで半二重回路を作ることもできます。
https://akizukidenshi.com/catalog/g/gI-15806/ など

拙作の"Meridian Board"も半二重回路を標準搭載していますので、気になる方は検索してみてください。

半二重回路は絶対必要か？

サーボがリターンパケットを返信しないようにするか、受信したリターンパケットを読み捨てるようにコードで制御すればなんとかなるかもしれません。（まだよくわかりません）
今回はライブラリをそのまま使うため、素直に半二重回路を使います。

ライブラリの導入

Dynamixel2Arduinoを使います。
Arduino IDEからの検索でインポートできます。

サーボとESP32および半二重回路との接続

サーボ線	機能	半二重回路	ESP32
外側の黒線	GND	GND	GNDピン
中央の黒線	Vcc	5V	5Vピン
外側の黒線	信号線	TX/RX	17番ピン(Serial2)
	切替	EN	4番ピン

サンプルコード

ESP32_dynamixel.ino

#include <Dynamixel2Arduino.h>

// サーボに送る信号についての初期設定
#define OPERATING_MODE_ADDR         11
#define OPERATING_MODE_ADDR_LEN     1
#define TORQUE_ENABLE_ADDR          64
#define TORQUE_ENABLE_ADDR_LEN      1
#define LED_ADDR                    65
#define LED_ADDR_LEN                1
#define GOAL_POSITION_ADDR          116
#define GOAL_POSITION_ADDR_LEN      4
#define PRESENT_POSITION_ADDR       132
#define PRESENT_POSITION_ADDR_LEN   4
#define POSITION_CONTROL_MODE       3 //位置の指示による制御モード
#define TIMEOUT 10    //サーボ信号のタイムアウト. デフォルトは10ms

// その他の初期設定
#define DXL_SERIAL   Serial2 // 使うシリアル系統
const uint8_t DXL_DIR_PIN = 4; // 半二重回路用のENピン
uint32_t centerPosition = 2048;//サーボ位置のセンター値
uint32_t goalPosition = 0;//サーボの目標位置
float radiansval = 0.0; // サインカーブ算出用のラジアン値
float radiansIncrement = 0.06; // ループ毎のラジアン値の増加量
float maxPosition = 2000;// センター値を2048としたとき、±どこまで振るか(0-2048)
const uint8_t DXL_ID = 1; //サーボID
const float DXL_PROTOCOL_VERSION = 2.0; //プロトコルのバージョン
uint8_t turn_on = 1;
uint8_t turn_off = 0;
uint8_t operatingMode = POSITION_CONTROL_MODE;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); //Dynamixel用ライブラリのインスタンス化

void setup() {
  dxl.begin(57600); // デフォルトのbaudrate. 必要に応じてサーボの設定にあわせる.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // サーボの初期設定時はトルクオフ
  if (dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
    led_on(); //成功ならLEDをオン
  else
    led_off(); //失敗ならLEDをオフ
    
  // サーボのOperating Modeを設定する
  if (dxl.write(DXL_ID, OPERATING_MODE_ADDR, (uint8_t*)&operatingMode, OPERATING_MODE_ADDR_LEN, TIMEOUT))
    led_on(); //成功ならLEDをオン
  else
    led_off(); //失敗ならLEDをオフ

  // サーボをトルクオン
  if (dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
    led_on(); //成功ならLEDをオン
  else
    led_off(); //失敗ならLEDをオフ
  delay(100);
}

void loop() {
  // サインカーブ用の値を算出
  radiansval += radiansIncrement;  //ラジアン値を増加
  radiansval = (radiansval > 2 * PI) ? 0 : radiansval; //ラジアン値が2πを超えたら0にリセット

  // サーボの値をセットする
  goalPosition = centerPosition + int(sin(radiansval) * maxPosition);
  // サーボにコマンドを送信
  dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition, GOAL_POSITION_ADDR_LEN, TIMEOUT);
  
  delay(20);
}

void led_on() { //LEDをオン
  dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT);
}

void led_off() { //LEDをオフ
  dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_off, LED_ADDR_LEN, TIMEOUT);
}

しくみ

サーボ内のマイコンに対して、どのアドレスにどの値を送るかというコマンドを作ってシリアルで送信しています。リターンパケットもライブラリで見ているようです。
アドレスと値の組み合わせで、サーボにさまざまなコマンドを送ることができます。

起動と動作結果

LEDが点灯し、1秒間隔ぐらいでサーボが加減速しながらブランコのように左右に回転すれば実験成功です。

トラブルシューティング

Dynamixelは起動時にいくつかルールがあるようです。
今回のサンプルプログラムでは、起動時の初期設定シーケンスに問題がある場合にはLEDを消灯するようになっています。
配線が合っているのにうまく起動しない場合は、ESP32の電源を入れ直し、リセットボタンを入れるとうまく動く場合があります。
原因はおそらく電力不足です。（簡易テストなのでUSBからESP32とサーボの両方を動かしています。）

さいごに

半二重回路を一つ持っているだけでArduino系のマイコンからいろいろなコマンドサーボが動かせるようになるのでおすすめです。

よき電子工作ライフを〜