回すだけ IQ2306 (3) コマンドを使う-2 位置決め

　ここでは、ブラシレスDCモータIQ2306の位置決めコマンドを扱います。

●Multi Turn Angle Control

　このコマンドの特徴です。

　位置決めを連続して複数の場所に設定して動くことができる制御を実装している。PIDのパラメータの設定ができる　
　回転から線形への計算コンバータが組み込まれているので、線形位置コントローラを模倣できる
　32セグメントのキューを備えた最小ジャーク軌道ジェネレータを搭載しているので、最小限の計算と通信オーバーヘッドで、滑らかで人間のような動きを生成できる

　用意されているコマンド一覧です。

Sub ID	Short Name	データ・タイプ	単位	備考
0	ctrl_mode	uint8	enum	no_change = -1, brake=0,coast=1, pwm=2, volts=3, velocity=4, angle=5, trajectory=6
1	ctrl_brake			モータのリード線をショートさせ、モータのエネルギーを消費してモータの速度を低下させる
2	ctrl_coast			すべての駆動回路を無効に。フリーにさせる
3	ctrl_angle	float	rad	角度位置コマンド
4	ctrl_velocity	float	rad/s	角速度コマンド
5	angle_Kp	float	V/rad	比例ゲイン
6	angle_Ki	float	V/(rad*s)	積分ゲイン
7	angle_Kd	float	V/(rad/s)	微分ゲイン
8	timeout	float	sec	コントローラはこの時間内にメッセージを受信する必要がある。そうしないと、コントローラーはコースト・モードに設定される
9	ctrl_pwm	float	pwm	このスロットルでモータを回転させる [-1,1]
10	ctrl_volts	float	V	この電圧でモータを回転させる
11	obs_angular_displacement	float	rad	観測された角度位置
12	obs_angular_velocity	float	rad/s	観測された角速度
13	meter_per_rad	float	m/rad
14	ctrl_linear_displacement	float	m	ctrl角度の線形等価
15	ctrl_linear_velocity	float	m/s	ctrl速度の線形等価
16	obs_linear_displacement	float	m	観測された線形位置
17	obs_linear_velocity	float	m/s	観測された線速度
18	angular_speed_max	float	rad/s
19	trajectory_angular_displacement	float	rad	軌跡の最終的な絶対変位
20	trajectory_angular_velocity	float	rad/s	軌道の最終速度。デフォルトは0
21	trajectory_angular_acceleration	float	rad/s^2	軌道の最終加速。デフォルトは0
22	trajectory_duration	float	sec	軌跡の持続時間。これが送信されると、軌道が実行またはキューに入れられる
23	trajectory_linear_displacement	float	m	軌道の最終的な絶対変位。デフォルトは0
24	trajectory_linear_velocity	float	m/s	軌道の最終速度。デフォルトは0
25	trajectory_linear_acceleration	float	m/s^2	軌道の最終加速。デフォルトは0

　Arduinoのサンプルでは、trajectory_angular_displacementを利用して位置決めを行っています。

●trajectory_angular_displacementのスケッチ

　このコマンドは「軌跡の最終的な絶対変位」を指定し、単位はラジアンradです。0度は0、180度はラジアンではπです。

#include <iq_module_communicaiton.hpp>

IqSerial ser(Serial1);
MultiTurnAngleControlClient mult(0);

float angle = 0;

void setup() {
    Serial.begin(9600); // USB-serial console
    while(!Serial);
        Serial.println("\nStart ");
    ser.begin(115200); // independent UART

    ser.set(mult.trajectory_angular_displacement_,0.0f);
    ser.set(mult.trajectory_duration_,0.5f);

    ser.get(mult.obs_angular_displacement_, angle);
    Serial.println("angle=" + String(angle) + "rad");

    Serial.println("=== end setup --- "); 
}

void loop() {
    delay(1000);
}

　絶対位置の0がどこか不明ですが、

　　ser.set(mult.trajectory_angular_displacement_,0.0f);

で動かします。その移動に必要な時間は、

　　ser.set(mult.trajectory_duration_,0.5f);

0.5秒にしました。その移動後、

　　ser.get(mult.obs_angular_displacement_, angle);

で角度を読み出します。

　実行結果です。

　loop()内で、180度と0度を往復するようにします。180度への移動は0.2秒で素早く、0度への復帰はゆっくりと動かします。

#include <iq_module_communicaiton.hpp>

IqSerial ser(Serial1);
MultiTurnAngleControlClient mult(0);

float angle = 0;

void setup() {
    Serial.begin(9600); // USB-serial console
    while(!Serial);
        Serial.println("\nStart ");
    ser.begin(115200); // independent UART

    ser.set(mult.trajectory_angular_displacement_,0.0f);
    ser.set(mult.trajectory_duration_,0.5f);

    ser.get(mult.obs_angular_displacement_, angle);
    Serial.println("angle=" + String(angle) + "rad");

    Serial.println("=== end setup --- "); 
}

void loop() {
    ser.set(mult.trajectory_angular_displacement_,3.14f);
    ser.set(mult.trajectory_duration_,0.2f);

    ser.get(mult.obs_angular_displacement_, angle);
    Serial.println("angle=" + String(angle) + "rad");

    ser.set(mult.trajectory_angular_displacement_,0.0f);
    ser.set(mult.trajectory_duration_,3.0f);

    ser.get(mult.obs_angular_displacement_, angle);
    Serial.println(" angle=" + String(angle*180.0/PI) + "degree");

    delay(1000);
}

　実行結果です。

　動画です。

VID_20190910_090756.mp4をダウンロード

　PIDは、三つのパラメータを使うことによって、より早く目的の位置に移動することができます。適切でなければ、モータが異常な振動をすることもあります。積分のパラメータKpを入れ、比例制御をします。

void setup() {
    Serial.begin(9600); // USB-serial console
    while(!Serial);
        Serial.println("\nStart ");
    ser.begin(115200); // independent UART

    ser.set(mult.angle_Kp_,0.9f);
    ser.set(mult.angle_Ki_,0.0f);
    ser.set(mult.angle_Kd_,0.0f);

    ser.set(mult.trajectory_angular_displacement_,0.0f);
    ser.set(mult.trajectory_duration_,0.5f);

    ser.get(mult.obs_angular_displacement_, angle);
    Serial.println("angle=" + String(angle) + "rad");

    Serial.println("=== end setup --- "); 
}

　実行結果です。

　P（Kp）とI（Ki）のパラメータを変更します。

void setup() {
    Serial.begin(9600); // USB-serial console
    while(!Serial);
        Serial.println("\nStart ");
    ser.begin(115200); // independent UART

    ser.set(mult.angle_Kp_,0.6f);
    ser.set(mult.angle_Ki_,0.3f);
    ser.set(mult.angle_Kd_,0.0f);

    ser.set(mult.trajectory_angular_displacement_,0.0f);
    ser.set(mult.trajectory_duration_,0.5f);

    ser.get(mult.obs_angular_displacement_, angle);
    Serial.println("angle=" + String(angle) + "rad");

    Serial.println("=== end setup --- "); 
}

　モータから異音が少し出ているので、適切な範囲を超えているのかもしれません。PIDのパラメータの変更で、180度の回転が正確になっている様子は見られません。それぞれのパラメータが適切でないと思われますし、もともとの移動時間も早すぎるのかもしれません。これらは、利用時の負荷によっても異なると思われます。数多くのデータ収集が必要でしょう。