Arduino UNO×SimpleFOCでブラシレスモータの位置制御をする

# はじめに
本記事では、Arduino UNOでブラシレスモータの位置制御に取り組みます。

ブラシレスモータの制御は、個人で一から取り組むのはハードルが高いです。そうした敷居を少しでも下げようとSimpleFOC[^1]というプロジェクトがあり、ブラシレスモータの代表的な制御ロジックであるFOC(Field Oriented Control[^2])のためのライブラリや、ドライバ基板が整備され、各種マイコンに対応しています。

[^1]: https://simplefoc.com/
[^2]: FOCは日本語ではなぜかベクトル制御と呼ばれることが多いです。

SimpleFOCについての日本語記事は少ないので、(サンプルプロジェクトを実行したレベルですが)本記事が誰かの参考になればと思います。間違いがあればコメント等で教えていただけると助かります。

![位置制御_short.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/f7d934df-21e9-0e97-b092-2e8bb4b1baf6.gif)


# サンプルプロジェクトとハードウェア構成

この記事で試すサンプルコードやハードウェア構成は↓にある内容です。

https://docs.simplefoc.com/position_control_example

画像のように、Arduino UNOにスタックされたモータドライバシールド(ドライバ基板)とモータ、エンコーダをつなぎ、位置制御をします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/be3dffaa-3bb5-34d5-1bda-529051cf0ca2.png)


ちなみにあとから気づいたのですが、このサンプルではいわゆるd軸、q軸の電流を制御しているわけではありません(その場合にもFOCと呼ぶ場合があるみたいです)。つまり、↓のドキュメントにある
https://docs.simplefoc.com/torque_control

↓の「FOC！」感のある構成(FOC current mode)ではなく
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/ddbbe41c-24d8-5488-b9d2-4583ff25daef.png)

↓の図(Voltage mode)となっています。もっと詳しいことを言うと、相抵抗R=1とみなして、速度制御器の出力から直接相電圧の指令を求め、回転子の角度(電気角)に応じた3相電圧への座標変換を行っているだけ、という構成です。
(Arduino UNOでやるには、特に電流検出周りのハード制約でこれが限界なんでしょうね。)
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/9af4fd69-c79e-ac58-1c6b-1acc4fef79e4.png)


# ハードウェアについて
今回使ったハードウェアについてまとめておきます。
| デバイス | 型番など | 入手方法 | 価格 | 備考|
| ---- | ---- | ---- | ---- | ---- |
| マイコン | Arduino UNO | ---- | 3,000円強 | 家にあった適当なやつ |
| 安定化電源 | Kungber SPS3010 | ---- | 7,000円強 | 家にあった。12Vぐらいが安定的に出力できればなんでもOK |
| モータドライバシールド | v2.0.4 | Aliexpress | 3,094円 | https://docs.simplefoc.com/arduino_simplefoc_shield_showcase v3.が最新で、基板デザインが公開されている。自作も可 |
| ブラシレスモータ | GM4108H-120T | Aliexpress | 3,457円 | https://shop.iflight.com/gimbal-motors-cat44/ipower-motor-gm4108h-120t-brushless-gimbal-motor-pro217 仕様書とかは無いようなので、磁極対数などはこのページの内容を参照した。 |
| ロータリエンコーダ | AMT103-V | Aliexpress | 4,447円 | https://www.mouser.jp/ProductDetail/CUI-Devices/AMT103-V?qs=WyjlAZoYn51X2GCrrvGQTg%3D%3D 仕様書はこのへんにある。 |


(価格は2024年7月時点で、参考程度に。送料とかは入ってないかも)

ちなみに、↓のサポートハードウェアの一覧に、先に示したPosition Control Exampleで使われているGM4108H-120Tは記載がありません。使えないのか？何か載ってない理由があるのか？ と不審に思いましたが、特に気にせずで大丈夫でした。

https://docs.simplefoc.com/bldc_motors

## モータ&エンコーダの取り付け
ブラシレスモータにエンコーダを取り付けるため、適当なマウント治具を作る必要があります。3Dプリンタがある人はそれで、ない人はどこかに作ってもらってください。

### モータ&エンコーダマウント
有志の方が、ぴったり良い感じに収まるやつを作ってくれています。
↓ エンコーダのねじ径がmount2.STLとmy-mount4.STLで異なります。私は前者を使うことにしました。エンコーダの取付穴はタップを切った方がいい気がしますが、自分の場合は上手いことはめ合ったので特にやっていません。

https://github.com/csunwc/simpleFOC-test

↑は↓この議論で参照されてる内容です。
https://community.simplefoc.com/t/ipower-gm4108h-120t-motor-mount-bracket-stl-file/2060

### モータシャフト
GM4108H-120Tは中空なので、シャフトを通してエンコーダとつなぐ必要があります。

径8mmシャフトですが、長さやはめ合いでいい感じのものが無かったので自作しました。
途中を8.2mmぐらいにしてはめ合うようにしてますが、太さは適宜調整ください。

https://github.com/SKMAAX/BLMotor_GM4108_Shaft

このシャフトとエンコーダの接続は、AMT103-Vに付属されている径8mm用のスリーブを使い、↓の7ページを参考に取付してください。

https://www.mouser.jp/datasheet/2/670/amt10_v-1775837.pdf

# ソフト環境構築
私の環境では、Win10上のVSCodeにてPlatformIOを使用しています。

未使用の方は、便利なのでこの機会にぜひ導入しましょう。PlatformIOがあれば、あれこれウインドウを開かずVSCode上でビルド・マイコンへの書き込みが完結します。

PlatformIOを入れた後、LibrariesからSimpleFOCを選んで「Add Project」とするとSimpleFOCの多種多様なライブラリが利用可能になります。

↓公式のドキュメントはこちら

https://docs.simplefoc.com/library_platformio

# ハードウェアの確認
やりたい制御をいきなり実行する前に、↓に沿ってピン設定や各ハードウェアの動作確認をしておきます。

- エンコーダの接続確認
- モータの接続確認
- モータの電圧制御での動作確認 → これは飛ばしました

https://docs.simplefoc.com/foc_shield_code

ハードウェアの動作確認用コードは、↓このようにPlatformIO上のExamplesから簡単にとってこれます。これらをmain.cppにコピペするとそのまま使えます。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/f01e5946-1498-1de2-6973-bac9c21ea07c.png)

## Arduino UNO特有のピン配置設定

SimpleFOCドライバシールドは、裏側のゼロオーム抵抗で端子設定が切り替えられるようになっています。

↓の記述を見て、Board#1の設定に合わせてはんだ付けしなおしました。

https://docs.simplefoc.com/pads_soldering_v2#customizing-pinout

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/2d36b428-c5c5-5d50-8e57-2b69167bf7b9.png)

が、結局自分のボードでは↓にしました。↑だとエンコーダA相が読めませんでした。詳しくはよくわかりませんがPWMのところのシルクの順番やその他も↑とは異なってますし、ご愛嬌と思うことにします。↓の黄色い印が要注意だった箇所です。
![IMG_3956.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/ca87b77b-1cf1-884e-1efb-00c4c98f5829.jpeg)

↑の点以外は特にハマることなく動作確認できました。

また、このモータの磁極対数は調べたところ11のようでした。
結局、対応するコード上の設定は↓としています。サンプルコードで設定する必要があるのはここだけです。

```C++
BLDCMotor motor = BLDCMotor( 11 );
BLDCDriver3PWM driver = BLDCDriver3PWM(10, 5, 6, 8);
Encoder encoder = Encoder(2, 3, 2048);
```

# 位置制御の実行
ハードウェアの確認が行えたら、いよいよ位置制御をしてみます。velocity_limitなどはデフォルトから多少上げたりしました。

↓のコードを実行します。
```C++:main.cpp
#include <SimpleFOC.h>

// init BLDC motor
BLDCMotor motor = BLDCMotor( 11 );
// init driver
BLDCDriver3PWM driver = BLDCDriver3PWM(10, 5, 6, 8);
//  init encoder
Encoder encoder = Encoder(2, 3, 2048);
// channel A and B callbacks
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

// angle set point variable
float target_angle = 0;
// commander interface
Commander command = Commander(Serial);
void onTarget(char* cmd){ command.scalar(&target_angle, cmd); }

void setup() {

  // initialize encoder hardware
  encoder.init();
  // hardware interrupt enable
  encoder.enableInterrupts(doA, doB);
  // link the motor to the sensor
  motor.linkSensor(&encoder);

  // power supply voltage
  // default 12V
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor to the driver
  motor.linkDriver(&driver);

  // set control loop to be used
  motor.controller = MotionControlType::angle;
  
  // controller configuration based on the control type 
  // velocity PI controller parameters
  // default P=0.5 I = 10
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;
  
  //default voltage_power_supply
  motor.voltage_limit = 6;

  // velocity low pass filtering
  // default 5ms - try different values to see what is the best. 
  // the lower the less filtered
  motor.LPF_velocity.Tf = 0.01;

  // angle P controller 
  // default P=20
  motor.P_angle.P = 20;
  //  maximal velocity of the position control
  // default 20
  motor.velocity_limit = 10;
  
  // initialize motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();

  // add target command T
  command.add('T', onTarget, "target angle");

  // monitoring port
  Serial.begin(115200);
  Serial.println("Motor ready.");
  Serial.println("Set the target angle using serial terminal:");
  _delay(1000);
}

void loop() {
  // iterative FOC function
  motor.loopFOC();

  // function calculating the outer position loop and setting the target position 
  motor.move(target_angle);

  // user communication
  command.run();
}
```

実行すると、勝手にモータが原点出し(というより、正確には電気角のオフセット値の推定)をしてくれます。その後、シリアルターミナル上に「Set the target angle using serial terminal:」と表示されるので、例えば0.5[rad]の位置に回転させたい場合はターミナル上で「T0.5」と打って送信します。このコマンドのフォーマットは↓に載っている「Scalar variables」に沿っています。
(他にもいろいろなコマンドが用意されているので、勝手が良さそうですね）

https://docs.simplefoc.com/commander_interface


このコードを実行して適当な指令角度を指定すると、↓こんな感じに動きます。

![位置制御_short.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/701220/f7d934df-21e9-0e97-b092-2e8bb4b1baf6.gif)

↓長尺版はこちら

https://x.com/SKMAAX/status/1814425794602774749

速度制御の方もほぼ同じ内容で、↓のサンプルコードを使用しています。
https://docs.simplefoc.com/velocity_control_example


# おわりに
SimpleFOCというプロジェクトで作られたライブラリやドライバ基板を使って、Arduino UNOでブラシレスモータの位置制御を行いました。Arduino UNOでブラシレスモータの位置制御ができるなんて思っても見なかったです。SimpleFOCに関する日本語記事は少ないですが、今後も発展していくといいですね。