ODrive入門　ーArduinoでBLDC制御

##はじめに
この記事は[ODrive 入門　ー初期設定から動作確認まで](https://qiita.com/ohaginia218/items/82bb377205fe2405a8b3)の続きです。

ODriveとROSとArduinoを使用している方が見つからず、情報があまりにも少ないため参考になれば幸いです。
本記事ではArduinoのシリアル通信でODriveを制御する方法を紹介します。

##ライブラリのインポート
ライブラリをインポートします。

このページからZIPでファイルをダウンロードします。
https://github.com/madcowswe/ODrive

ファイルを解凍して、Arduino IDEを起動します。

>スケッチ -> ライブラリのインクルード -> .ZIP形式のライブラリをインストール

```
解凍したディレクトリ / ODrive-master / Arduino / ODriveArduino
```
を選択し、インポート完了

##配線
| Arduino MEGA | ODrive |
|:-:|:-:|
| RX2 17  | GPIO 1  |
| TX2 16  | GPIO 2  |
| GND     |  GND    |

##Arduinoスケッチ例

>ファイル -> スケッチ例 -> ODriveArduino -> ODriveArduinoTest

```cpp:ODriveArduinoTest.ino

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

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

// Serial to the ODrive
SoftwareSerial odrive_serial(8, 9); //RX (ODrive TX), TX (ODrive RX)
// Note: you must also connect GND on ODrive to GND on Arduino!

// 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

  Serial.println("ODriveArduino");
  Serial.println("Setting parameters...");

  // In this example we set the same parameters to both motors.
  // You can of course set them different if you want.
  // See the documentation or play around in odrivetool to see the available parameters
  for (int axis = 0; axis < 2; ++axis) {
    odrive_serial << "w axis" << axis << ".controller.config.vel_limit " << 22000.0f << '\n';
    odrive_serial << "w axis" << axis << ".motor.config.current_lim " << 11.0f << '\n';
    // This ends up writing something like "w axis0.motor.config.current_lim 10.0\n"
  }

  Serial.println("Ready!");
  Serial.println("Send the character '0' or '1' to calibrate respective motor (you must do this before you can command movement)");
  Serial.println("Send the character 's' to exectue test move");
  Serial.println("Send the character 'b' to read bus voltage");
  Serial.println("Send the character 'p' to read motor positions in a 10s loop");
}

void loop() {

  if (Serial.available()) {
    char c = Serial.read();

    // Run calibration sequence
    if (c == '0' || c == '1') {
      int motornum = c-'0';
      int requested_state;

      requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION;
      Serial << "Axis" << c << ": Requesting state " << requested_state << '\n';
      odrive.run_state(motornum, requested_state, true);

      requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
      Serial << "Axis" << c << ": Requesting state " << requested_state << '\n';
      odrive.run_state(motornum, requested_state, true);

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

    // Sinusoidal test move
    if (c == 's') {
      Serial.println("Executing test move");
      for (float ph = 0.0f; ph < 6.28318530718f; ph += 0.01f) {
        float pos_m0 = 20000.0f * cos(ph);
        float pos_m1 = 20000.0f * sin(ph);
        odrive.SetPosition(0, pos_m0);
        odrive.SetPosition(1, pos_m1);
        delay(5);
      }
    }

    // Read bus voltage
    if (c == 'b') {
      odrive_serial << "r vbus_voltage\n";
      Serial << "Vbus voltage: " << odrive.readFloat() << '\n';
    }

    // print motor positions in a 10s loop
    if (c == 'p') {
      static const unsigned long duration = 10000;
      unsigned long start = millis();
      while(millis() - start < duration) {
        for (int motor = 0; motor < 2; ++motor) {
          odrive_serial << "r axis" << motor << ".encoder.pos_estimate\n";
          Serial << odrive.readFloat() << '\t';
        }
        Serial << '\n';
      }
    }
  }
}
```

１０行目をコメントアウトして、odrive_serial -> Serial2　に置換する。

```cpp:ODriveArduinoTest.ino(10行目)
SoftwareSerial odrive_serial(8, 9); //RX (ODrive TX), TX (ODrive RX)
```

そして新規ファイルで保存。

```cpp:ODriveArduinoMyTest.ino

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

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

// Serial to the ODrive
//SoftwareSerial Serial2(8, 9); //RX (ODrive TX), TX (ODrive RX)
// Note: you must also connect GND on ODrive to GND on Arduino!

// ODrive object
ODriveArduino odrive(Serial2);

void setup() {
  // ODrive uses 115200 baud
  Serial2.begin(115200);

  // Serial to PC
  Serial.begin(115200);
  while (!Serial) ; // wait for Arduino Serial Monitor to open

  Serial.println("ODriveArduino");
  Serial.println("Setting parameters...");

  // In this example we set the same parameters to both motors.
  // You can of course set them different if you want.
  // See the documentation or play around in odrivetool to see the available parameters
  for (int axis = 0; axis < 2; ++axis) {
    Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 22000.0f << '\n';
    Serial2 << "w axis" << axis << ".motor.config.current_lim " << 11.0f << '\n';
    // This ends up writing something like "w axis0.motor.config.current_lim 10.0\n"
  }

  Serial.println("Ready!");
  Serial.println("Send the character '0' or '1' to calibrate respective motor (you must do this before you can command movement)");
  Serial.println("Send the character 's' to exectue test move");
  Serial.println("Send the character 'b' to read bus voltage");
  Serial.println("Send the character 'p' to read motor positions in a 10s loop");
}

void loop() {

  if (Serial.available()) {
    char c = Serial.read();

    // Run calibration sequence
    if (c == '0' || c == '1') {
      int motornum = c-'0';
      int requested_state;

      requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION;
      Serial << "Axis" << c << ": Requesting state " << requested_state << '\n';
      odrive.run_state(motornum, requested_state, true);

      requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
      Serial << "Axis" << c << ": Requesting state " << requested_state << '\n';
      odrive.run_state(motornum, requested_state, true);

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

    // Sinusoidal test move
    if (c == 's') {
      Serial.println("Executing test move");
      for (float ph = 0.0f; ph < 6.28318530718f; ph += 0.01f) {
        float pos_m0 = 20000.0f * cos(ph);
        float pos_m1 = 20000.0f * sin(ph);
        odrive.SetPosition(0, pos_m0);
        odrive.SetPosition(1, pos_m1);
        delay(5);
      }
    }

    // Read bus voltage
    if (c == 'b') {
      Serial2 << "r vbus_voltage\n";
      Serial << "Vbus voltage: " << odrive.readFloat() << '\n';
    }

    // print motor positions in a 10s loop
    if (c == 'p') {
      static const unsigned long duration = 10000;
      unsigned long start = millis();
      while(millis() - start < duration) {
        for (int motor = 0; motor < 2; ++motor) {
          Serial2 << "r axis" << motor << ".encoder.pos_estimate\n";
          Serial << odrive.readFloat() << '\t';
        }
        Serial << '\n';
      }
    }
  }
}

```
さぁ実行してみましょう。

シリアルモニタから"0"を送信をするとモータが"0"がイニシャライズされ、sで余弦波１周期分、位置制御されます。
bで電圧の表示、pでエンコーダ位置の表示などが行えると思います。


それでは中身の説明をしていきます。
スケッチを見ても分かる通り、ODriveArduinoは概ねASCIIプロトコルでコマンドを送受信する形でプログラムが書かれてます。

ASCII プロトコルについて軽く説明したいと思います。

まず、ODriveオブジェクトを作成します。

```cpp:
// ODrive object
ODriveArduino odrive(Serial2);
```
そして作成されたオブジェクトに対して挿入子で読み書きしていく感じです。

書き込みコマンドの一例です。

```cpp:
Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 22000.0f << '\n';
```
頭にw(write)をつけ、次にモータ番号、指令コマンド、指令値を与えます。


読み取りコマンドの一例です。

```cpp:
Serial2 << "r axis" << motor << ".encoder.pos_estimate\n";
Serial << odrive.readFloat() << '\t';
```
頭にr(read)をつけ、次にモータ番号、指令コマンドを与えます。
そして、```odrive.readFloat()``` または```odrive.readInt()```で値を取得します。

これを押さえとけば大体はプログラム書けるのではないでしょうか！
今回はシリアルモニタを使っての通信でしたが次回はROSを使っての通信を紹介します。

##参考文献

https://docs.odriverobotics.com/ascii-protocol.html