C言語でEV3開発(21)-モーター出力の制御

2021年2月6日

どもです。
前回は、超音波センサから前方障害物との距離を取得し、それを元に安全状態を判断する、という内容について書きました。
今回は、前回の内容を踏まえて、モーターの出力値を制御する方法について記載します。

1.モーターの設定

モーターを設定、出力値(動作させたい値)を指定、あるいは動作値(現在の出力値)は、やっぱりEV3RTのAPIを使用します。
詳細は、コチラに掲載されているので、このエントリーで詳細は記載しません。
必要に応じて、参照して下さい。
モーターの特性については、コチラからダウンロードできる資料を参考にして下さい。
また、コチラのサイトも参考になるかも知れませんので、リンクを張っておきます。

2.モーターの目標出力値の指定

モーターを動かす/制御するに当たり、「目標となるモーターの出力値」を指定する必要があります。
これは、前回までのエントリーで紹介してきた、Bluetooth通信を用いたコマンド処理により行います。
また、単にモーターの出力を指定するだけではなく、ステアリング比(こんな言葉があるのか？)もコマンドで指定するようにします。
モーターの出力値を指定するためのコマンドは、以下のように設計しました。
SetMotorPower_CmdFrmt
SetSteer_CmdFrmt

3.モーター出力値の算出
コマンドで目標モーター出力値を受信してから実際にモーターにその値を通知するまでは、下記のような順で処理を実行します。
SoftwareArchitecture_DriveMotor
この構成に従ったモーター制御の処理順の実装は、下記の通りです。

void motor_task(intptr_t unused) {
    while (motor_task_running) {
        //Read current, actual motor output.
        motor_get_power();

        //Calcurate motor output.
        judge_motor_output_limit();
        judge_target_motor_output();
        calc_target_motor_outputLR();
        calc_motor_power();

        //Set motor output data.
        motor_set_power();

        motor_task_count++;
        dly_tsk(MOTOR_TASK_INTERVAL);
    }
}

※MOTOR_TASK_INTERVALは、10msecを設定しています。

モーター出力確定では、フィードバック制御を用いて、最終的なモーター出力値を算出します。
とくに、フィードバック制御では、PID制御を適用します。
PID制御を適用した理由ですが、以前PID制御を用いたモーター制御について検討したことがあるためです。
(詳しくは、コチラの記事を参照してください。)
具体的には、以下のようにPID制御を実装しています。

void calc_motor_power(void) {
    int left_output = 0;
    int right_output = 0;

    left_power.target = target_motor_output_left;
    right_power.target = target_motor_output_right;
    
    left_power.current = left_motor_power_current;
    right_power.current = right_motor_power_current;

    left_output = pid_control(&left_power);
    right_output = pid_control(&right_power);

    /**
     *  Calcurate Finalized motor power.
     *  Due to Hardware or OS (maybe OS), the motor can not drive with
     *  the absolute actual motor power with 50. If the power value exceeds
     *  50 is set, the motor dirve does not change.
     *  And, the case that the value exceeds 50 is set to motor, sometime
     *  the motor can not operate in a precise manner. To avoid this situation,
     *  limits motor output preliminarily.
     */
    //Motor output power(Limitation).
    //Left side motor.
    left_motor_power = limit_int(left_output,
        MOTOR_POWER_MIN, MOTOR_POWER_MAX);
    //Right side motor.
    right_motor_power = limit_int(right_output,
        MOTOR_POWER_MIN, MOTOR_POWER_MAX);
}


/**
 *  @brief  Execute PID control.
 *  @param[in,out]  Value for PID control.
 *  @return Calcurated amount by PID control.
 */
static int pid_control(power_t *power) {
    int ddt = 0;    //derivative element
    int diff = 0;
    int pid = 0;
    int integral = 0;

    if (NULL != power) {
        diff = power->target - power->current;

        integral = power->integral + diff;
        ddt = power->diff - diff;
        pid = (Kp * diff + Ki * integral + Kd * ddt) / 100;

        power->target_prev = power->target;
        power->integral = integral;
        power->diff = diff;
    } else {
        pid = 0;
    }

    return pid;
}

※他の処理(モーター出力の最大値の判定)の実装は、このエントリーでは割愛します。

4.Ev3の車に載せて動かしてみた

上記の通りに実装したアプリケーションを使用してEv3を動作させた際の様子が、以下の動画です。

…おかしいですね…。
期待した動きとしては、
「まっすぐ進んで、壁の直前で停止して、バックして、またまっすぐ進んで…」
という動きを繰り返すように動かしたつもりなんですが…。
ハンドル操作(？)を間違えたかな？
それとも、バグ！？
とりあえず、動きをロギングできていないので、原因は闇の中になってしまいました。

5.まとめ

今回のエントリー/動画では、上記で実装したPID制御とそれによる車の制御を見せたかったのですが、何やら目的と違うエントリーになってしまいました。
無念です。
あ。
前回のエントリーで書いた、超音波センサから取得した前方の障害物との距離から安全状態を判定する、という機能は正しく動作していることだけは確認できました。

次回は、もう少し、まともなエントリーにします。
ではっ！

ev3,開発

Posted by CountrySideEngineer