ODriveでBLDCモーターの制御 #03 Arduino

前回、GUIでテスト動作が確認できたODriveですが、今回は外部のプログラムからODriveの位置制御を行いたいと思います。

Githubを見ると、Arduinoを用いたasciiプロトコルのサンプルコードが軽量で使いやすそうだったので、まずはArduinoからODriveにコマンドを送って制御するのを試してみたいと思います。

あと、追加要素としてPS2コントローラーも繋いでみました。

Arduino UNOとODriveの接続は、

• Arduino D8 – ODrive GPIO1
• Arduino D9 – ODrive GPIO2
• Arduino GND – ODrive GND

という感じに接続しています。

使い方としては、GithubのArduinoライブラリをArduino IDEに読み込ませると、Arduino IDEのサンプルにODriveArduinoTestが追加されます。
ライブラリを読み込ませる説明はこちらに書かれています。

使用するArduinoに合わせて、「Set up serial pins to the ODrive」の部分をコメントアウト／コメント解除する必要があります。UNOの場合は、UNOのところをコメント解除します。

まずは、サンプルをそのまま書き込んだところです。

シリアルコンソールから

• “0”を送ると、キャリブレーション
• “s”を送ると、サンプル動作
• “b”を送ると、電源電圧の表示
• “p”を送ると、現在位置の表示

が実行されます。

リセットが実装されていないため、ODriveがエラーを吐いた際は、ODriveの電源を再投入して再起動する必要があります。

PS2コントローラーでODriveを制御する

次にPS2コントローラーで制御します。
ODriveArduinoTestにはリセットコマンドがなかったので、それも送れるようにしました。

PS2コントローラーの取得は、Arduino用PS2インターフェース・ライブラリを使わせていただきました。

Arduino用PS2インターフェース・ライブラリは、少し古いので、GPSX.hの
「#include <wiring.h>」をコメントアウトして、「#include <Arduino.h>」に書き換える必要があるところだけ、注意が必要です。

動作する様子はこちらです。

PS2コントローラーにて、

• 〇を押すと、 0.0f の位置に
• □を押すと 10.0f の位置に
• ×を押すとリセット
• △を押すとキャリブレーション＆クローズドループモード
• L1ボタンを押している間、左アナログスティックで制御

という感じになっています。

ドキュメントを読んでもピンと来てなかったのですが、サンプルソースを読んだらODriveのasciiプロトコルはかなり簡単仕様で、改造もしやすい感じでした。
プロトコル的にエラーとか何も考えてない感じはありますが。

どちらかというと、前回同様にやはりODriveの設定が大変で、動作時のパラメータが設定リミットに触れるとすぐにエラー状態になって、ODriveが停止してしまいます。エラーを回避するパラメータの決定と対策が大変そうですね。

次回は、Arduinoではなく、PCから直接asciiコマンドを送ってみたいと思います。PCからODriveを制御するアプリに関しても、考えてみようと思います。

せっかくなのでPSコントローラーでODriveを制御するソースコード、貼っておきます。

#include <GPSXClass.h>
#include <HardwareSerial.h>
#include <SoftwareSerial.h>
#include <ODriveArduino.h>

// Printing with stream operator helper functions
template<class T> inline Print&amp; operator <<(Print &amp;obj,     T arg) { obj.print(arg);    return obj; }
template<>        inline Print&amp; operator <<(Print &amp;obj, float arg) { obj.print(arg, 4); return obj; }

////////////////////////////////
// Set up serial pins to the ODrive
////////////////////////////////

// Below are some sample configurations.
// You can comment out the default Teensy one and uncomment the one you wish to use.
// You can of course use something different if you like
// Don't forget to also connect ODrive GND to Arduino GND.

// Teensy 3 and 4 (all versions) - Serial1
// pin 0: RX - connect to ODrive TX
// pin 1: TX - connect to ODrive RX
// See https://www.pjrc.com/teensy/td_uart.html for other options on Teensy
//HardwareSerial&amp; odrive_serial = Serial1;

// Arduino Mega or Due - Serial1
// pin 19: RX - connect to ODrive TX
// pin 18: TX - connect to ODrive RX
// See https://www.arduino.cc/reference/en/language/functions/communication/serial/ for other options
// HardwareSerial&amp; odrive_serial = Serial1;

// Arduino without spare serial ports (such as Arduino UNO) have to use software serial.
// Note that this is implemented poorly and can lead to wrong data sent or read.
// pin 8: RX - connect to ODrive TX
// pin 9: TX - connect to ODrive RX
 SoftwareSerial odrive_serial(8, 9);

// ODrive object
ODriveArduino odrive(odrive_serial);

void setup(){
  
  // ODrive uses 115200 baud
  odrive_serial.begin(115200);
  
  // Serial to PC
  Serial.begin(115200);
  while (!Serial) ; // wait for Arduino Serial Monitor to open

  //PSX init
  PSX.mode(PSX_PAD1, MODE_ANALOG, MODE_LOCK);
  PSX.motorEnable(PSX_PAD1, MOTOR1_ENABLE, MOTOR2_ENABLE);
  // Poll current state once.
  PSX.updateState(PSX_PAD1);
  
  Serial.println("ODriveArduino");
  Serial.println("Setting parameters...");
  for (int axis = 0; axis < 2; ++axis) {
    odrive_serial << "w axis" << axis << ".controller.config.vel_limit " << 30000.0f << '\n';
    odrive_serial << "w axis" << axis << ".motor.config.current_lim " << 11.0f << '\n';
  }
  Serial.println("Ready!");
}

void loop(){
  
  float pos_m0 = 0.0f ;
  int requested_state;
  int motornum = 0;

  PSX.updateState(PSX_PAD1);

  // 〇が押されたら 0.0f の位置に
  if (PRESSED_CIRCLE(PSX_PAD1)) {
    Serial.println("Pressed circle");
    pos_m0 = 0.0f ;
    odrive.SetPosition(0, pos_m0);
  }

  // □が押されたら 10.0f の位置に
  if (PRESSED_SQUARE(PSX_PAD1)) {
    Serial.println("Pressed square");
    pos_m0 = 10.0f ;
    odrive.SetPosition(0, pos_m0);
  }

  // ×が押されたらリセット
  if (PRESSED_CROSS(PSX_PAD1)) {
    Serial.println("Pressed cross");
    odrive_serial << "sr\n";
    Serial << "rest odrive" << '\n';
  }

  // △が押されたらキャリブレーション＆クローズドループモード
  if (PRESSED_TRIANGLE(PSX_PAD1)) {
    Serial.println("Pressed triangle");
      requested_state = ODriveArduino::AXIS_STATE_FULL_CALIBRATION_SEQUENCE;
      Serial << "Axis" << "0" << ": Requesting state " << requested_state << '\n';
      if(!odrive.run_state(motornum, requested_state, true, 25.0f)) return;

      requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL;
      Serial << "Axis" << "0" << ": Requesting state " << requested_state << '\n';
      if(!odrive.run_state(motornum, requested_state, false /*don't wait*/)) return;
  }

  // L1ボタンを押している間、左アナログスティックで制御
  if (IS_DOWN_L1(PSX_PAD1)) {
    pos_m0 = (ANALOG_LEFT_Y(PSX_PAD1) - 127) / 5;
    odrive.SetPosition(0, pos_m0);
    Serial << "Pos: " << pos_m0 << '\n';
  }
}