RMD-X10をM5 Stackで動かす(2)

前回は動作確認まで完了しました．
今回はモーターを動かすところまで進みましょう．
サンプルプログラムは https://github.com/t-kamimura/RMDX10_m5stack にあります．該当するプログラム名はそれぞれの詳細説明の中にあります．

# プロトコル

RMD-X10は標準フォーマットのCAN通信を用いています．

予め定められた形式で8バイトの情報を送信すると，それに応じて動作を行います．よく使う機能としては，

* PIDゲインの設定
* サーボの角度ゼロ設定
* モーター角度の読み込み
* 位置制御指令

などがあります．他にも速度制御，電流制御などが可能です．

よく使うコマンドを抜き出したものが以下になります．

|  指令  |  data[0]  |  data[1]  |  data[2]   |  data[3]  | data[4]  | data[5]  |  data[6]  |  data[7] |
| ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- |
|Read PID                   | 0x30	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00|
|Write PID to RAM	        | 0x31	|0x00	|angle PID KP	|angle PID KI	|speed PID KP	|speed PID KI	|current PID KP	|current PID KI|
|Read multi turn angle	    | 0x92	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00|
|(reply) Read multi angle	| 0x92  | 0x00	| 0x00	| 0x00	| angle low byte 1 	|angle byte 2	|angle byte 3	|angle byte 4|
|Motor stop command	        | 0x81	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00|
|Position control command	| 0xA4	|0x00	|speed limit low byte	|speed limit high byte	|position low byte 1	|position byte 2	|position byte 3	|position high byte 4|

:::note warn
モータードライバV3ではMulti turn angleが64ビットから32ビットに縮小されました．RMD-X8について書いた以前の記事( https://qiita.com/tomoyakamimura/items/0b3fc70a5f1bed691412 )から変更されていますので注意してください．
:::

# 角度の読み込み

前回のプログラムはPIDゲインを読み取るだけでしたが，今回はモーター角度を読み取ってみましょう．

角度読み込みのサンプルプログラムは`RMD_ReadPosTest`です．

モーターにはエンコーダが取り付けられており，内部のマイコンが軸の回転数を計測してくれるという機能が備わっています．
"Read multi turn angle"というやつがそれです．単位は 1 LSD = 0.01 deg となっていて，すなわち返信値が3600になったときに軸が360 deg 回転したことになります．
モーターは35:1の減速比になっていますが，このコマンドは出力軸の角度そのものを教えてくれるようになっており，歯車のことを気にする必要はありません．

まず，角度読み込みのプロトコルは以下のとおりです．

|  指令  |  data[0]  |  data[1]  |  data[2]   |  data[3]  | data[4]  | data[5]  |  data[6]  |  data[7] |
| ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- |
|Read multi turn angle	    | 0x92	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00	|0x00|

これに対して，モーターから以下のような返信が送られてきます．

|  data[0]  |  data[1]  |  data[2]   |  data[3]  | data[4]  | data[5]  |  data[6]  |  data[7] |
| ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- |
|0x92   | 0x00	| 0x00	| 0x00	| angle byte 1 	| angle byte 2	| angle byte 3	|angle byte 4|

角度データは`int32_t`型で作成され，それが1バイトごとに分割されて送られてきます．
そこで，読み込んだデータをつなぎ合わせて本来のデータに戻してやりましょう．
モーターの読み込みには本来のデータ型と同じ`int32_t`型の変数を用意してやります．
![bitShift_pos.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/669185/ef2fa660-0b40-52e6-afc6-30760e72cab6.png)

サンプルプログラムの重要なところを解説しておきましょう．
まず，32ビットの整数型変数`present_pos`と，小数型変数`angle`を準備しておきます．
```c
int32_t present_pos = 0;
double angle = 0.0;
```

CAN通信で`0x92`で始まるRead multi angleコマンドを送ります．すなわち，
```c
// read multi turn angle
unsigned char cmd_buf[8]
cmd_buf[0] = 0x92;
cmd_buf[1] = 0x00;
cmd_buf[2] = 0x00;
cmd_buf[3] = 0x00;
cmd_buf[4] = 0x00;
cmd_buf[5] = 0x00;
cmd_buf[6] = 0x00;
cmd_buf[7] = 0x00;
```
というコマンド配列を作成し，`write_can()`関数でモーターに送ります．
その後，`read_can()`関数で返信を受け取ります．その後，以下のようにして受け取ったデータを意味のある数値に変換します．

```c
present_pos = reply_buf[4] + (reply_buf[5] << 8) + (reply_buf[6] << 16) + (reply_buf[7] << 24);
angle = present_pos * 0.01;
```

`<<`はビット演算子です．指定した数だけビットを動かしてやるということです．
最後の行では，読み込んだ値をモーターの出力軸角度に変換しています．
これでモーターの角度が読み込めるようになりました．

# 位置指令

RMD-X8には4種類の位置指令コマンドが用意されています．
便利なのは複数回転対応の位置指令Absolute position closed-loop commandでしょう．

:::note warn
モータードライバV3では，速度上限を設定しない位置制御である`0xA3`コマンドが廃止されました．RMD-X8について書いた以前の記事( https://qiita.com/tomoyakamimura/items/0b3fc70a5f1bed691412 )から変更されていますので注意してください．
:::

Position commandのサンプルプログラムは`RMD_movetest`です．

|  指令  |  data[0]  |  data[1]  |  data[2]   |  data[3]  | data[4]  | data[5]  |  data[6]  |  data[7] |
| ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- |
|Position control command	| 0xA4	| 0x00	| speed limit low byte	| speed limit high byte	| position low byte 1	| position byte 2	| position byte 3	|position byte 4|


位置指令は`int32_t`型で作成します．
角度の単位は角度読み取りのときと同じ定義で，1 LSB = 0.01 deg となります．
CANで送るときは，32ビット(=4バイト)で表された位置を4つのバイトに分割してやります．

```c
int tgt_angle = 4500; //[0.01 deg]
pos_byte[0] = pos_target & 0xFF;
pos_byte[1] = (pos_target >> 8) & 0xFF;
pos_byte[2] = (pos_target >> 16) & 0xFF;
pos_byte[3] = (pos_target >> 24) & 0xFF;
```

念のために，右シフトした後，下から8ビットより上はすべてゼロになるように`$ 0xFF`としています．
（`0xFF = 0b00000000`）

速度上限指令は`int16_t`型で作成します．
速度の単位は1 LSB = 1 deg/sとなります．
CANで送るときは，16ビット(=2バイト)で表された位置を2つのバイトに分割してやります．

```c
int max_vel = 30;   //[deg/s]
vel_byte[0] = max_vel & 0xFF;
vel_byte[1] = (max_vel >> 8) & 0xFF;
```

上記データをつなぎ合わせて，以下のように位置指令コマンドを作成します．
```c
byte data[8] = {0xA4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
data[2] = vel_byte[0];
data[3] = vel_byte[1];
data[4] = pos_byte[0];
data[5] = pos_byte[1];
data[6] = pos_byte[2];
data[7] = pos_byte[3];
```
これを`write_can()`関数で送ってやると，モーターが指令した角度まで回転します．

これでモーターの位置制御もできるようになりました．