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

モータードライバ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型の変数を用意してやります．

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

int32_t present_pos = 0;
double angle = 0.0;

CAN通信で0x92で始まるRead multi angleコマンドを送ります．すなわち，

// 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()関数で返信を受け取ります．その後，以下のようにして受け取ったデータを意味のある数値に変換します．

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でしょう．

モータードライバ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つのバイトに分割してやります．

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つのバイトに分割してやります．

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

上記データをつなぎ合わせて，以下のように位置指令コマンドを作成します．

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()関数で送ってやると，モーターが指令した角度まで回転します．

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