こんにちは。koraです。
ジャイロやエンコーダの値を反映してマウスを自律走行をできるようにしたいので、今回はサンプルプログラムでどのようになっているのか確認します。
サンプルプログラムの確認
割り込み
まず、サンプルプログラムStep7のinterrupt.cファイルのint_cmt0関数を見てみましょう。これはタイマ割り込みによって1msおきに実行される関数です。最初のif文によって、グローバル変数run_modeごとに、実行される内容が異なることがわかります。
簡単のため、まずはrun_modeがSTRAIGHT_MODEの場合に着目します。これは台形加速（一定の加速度で加速した後、一定の速度で走行し、最後に一定の加速度で減速する走り方）で直進するモードです。
1.目標速度生成
tar_speed += accel/1000.0; //目標速度を設定加速度で更新
//最高速度制限
if(tar_speed > max_speed){
tar_speed = max_speed; //目標速度を設定最高速度に設定
}
目標速度tar_speed [m/s]に加速度accel [m/s^2]を加算して、徐々に加速します。ちなみに、加速度accel [m/s^2] を1000で割っているのは、この処理が1秒間に1000回実施されるからです。
2.横壁センサによる目標角度生成
if(con_wall.enable == true && sen_fr.value + sen_fl.value <= (TH_SEN_FR+TH_SEN_FL)*5 )
{
con_wall.p_error = con_wall.error;
//左右のセンサが、それぞれ使える状態であるかどうかチェックして、姿勢制御の偏差を計算
if( ( sen_r.is_control == true ) && ( sen_l.is_control == true ) )
{ //両方とも有効だった場合の偏差を計算
con_wall.error = sen_r.error - sen_l.error;
}
else //片方もしくは両方のセンサが無効だった場合の偏差を計算
{
con_wall.error = 2.0 * (sen_r.error - sen_l.error); //片方しか使用しないので2倍する
}
//DI制御計算
con_wall.diff = con_wall.error - con_wall.p_error; //偏差の微分値を計算
con_wall.sum += con_wall.error; //偏差の積分値を計算
if(con_wall.sum > con_wall.sum_max) //偏差の積分値の最大値を制限
{
con_wall.sum = con_wall.sum_max;
}
else if(con_wall.sum < (-con_wall.sum_max)) //偏差の積分値の最低値を制限
{
con_wall.sum = -con_wall.sum_max;
}
con_wall.p_omega = con_wall.omega;
con_wall.omega = con_wall.kp * con_wall.error * 0.5 + con_wall.p_omega * 0.5; //現在の目標角速度[rad/s]を計算
tar_ang_vel = con_wall.omega;
}else{
tar_ang_vel = 0;
}
壁制御をONに設定している場合、割り込みint_cmt1関数で更新される光センサの値sen_r.value、sen_l.valueをもとに、目標角速度tar_ang_vel [rad/s]を生成します。なお、右壁が無い場合sen_r.errorに0が、左壁が無い場合sen_l.errorに0が入るので、両壁が無い場合は壁制御の影響が無くなります。
3.目標速度と目標角角度の積分
I_tar_speed += tar_speed;
if(I_tar_speed >30*10000000000){
I_tar_speed = 30*10000000000;
}else if(I_tar_speed < -1*10000000000){
I_tar_speed = 1*10000000000;
}
I_tar_ang_vel += tar_ang_vel;
if(I_tar_ang_vel >30*10000000000){
I_tar_ang_vel = 30*10000000000;
}else if(I_tar_ang_vel < -1*10000000000){
I_tar_ang_vel = 1*10000000000;
}
I_tar_speedとI_tar_ang_velは、それぞれ目標速度の積分値と目標角速度の積分値です。積分値が大きくなりすぎないようになっています。
4.目標速度の偏差から出力電圧にフィードバック
//左右のトルクフィードバック //速度に対するP制御 V_r += 1 * (tar_speed - speed) *SPEED_KP/1.0; //15目標値付近で発振 V_l += 1 * (tar_speed - speed) *SPEED_KP/1.0; //速度に対するI制御 V_r += 1 * (I_tar_speed - I_speed) *SPEED_KI/1.0; //(0.4-0.3)*0.1 -> 0.01 V_l += 1 * (I_tar_speed - I_speed) *SPEED_KI/1.0; //速度に対するD制御 V_r -= 1 * (p_speed - speed) *SPEED_KD/1.0; //(0.4-0.3)*0.1 -> 0.01 V_l -= 1 * (p_speed - speed) *SPEED_KD/1.0; //角速度に対するP制御 V_r += 1 * (tar_ang_vel - ang_vel) *(OMEGA_KP/100.0); V_l -= 1 * (tar_ang_vel - ang_vel) *(OMEGA_KP/100.0); //角速度に対するI制御 V_r += 1 * (I_tar_ang_vel - I_ang_vel) *(OMEGA_KI/100.0); //(0.4-0.3)*0.1 -> 0.01 V_l -= 1 * (I_tar_ang_vel - I_ang_vel) *(OMEGA_KI/100.0); //角速度に対するD制御 V_r += 1 * (p_ang_vel - ang_vel) *(OMEGA_KD/100.0); //(0.4-0.3)*0.1 -> 0.01 V_l -= 1 * (p_ang_vel - ang_vel) *(OMEGA_KD/100.0);
目標速度と現在の速度の差、および目標角速度と目標角速度の差をもとに、PID制御で出力電圧を計算しています。
5.出力電圧からモータの回転方向を決定
//右モータの出力電圧が正の場合
if(V_r > 0){
//モータを正転に設定
MOT_CWCCW_R = MOT_R_FORWARD; //右モータを正転に設定
//V_r = V_r; //電圧は正なのでそのまま
}else{
//右モータの出力電圧が負の場合
MOT_CWCCW_R = MOT_R_BACK; //右モータを逆回転に設定
V_r = -V_r; //電圧を正の値へ反転
}
//左モータの出力電圧が正の場合
if(V_l > 0){
//モータを正転に設定
MOT_CWCCW_L = MOT_L_FORWARD; //左モータを正転に設定
//V_l = V_l; //電圧は正なのでそのまま
}else{
//左モータの出力電圧が負の場合
MOT_CWCCW_L = MOT_L_BACK; //左モータを逆回転に設定
V_l = -V_l; //電圧を正の値へ反転
}
モータドライバには、回転方向と電圧をそれぞれ別に指定する仕様になっているので、計算した電圧をもとに反映します。
6.出力電圧の制限
//右モータに印加する電圧が2.0Vを超えたら強制的に2.0Vに変更
if(V_r > 2.0){
V_r = 2.0;
}
//左モータに印加する電圧が2.0Vを超えたら強制的に2.0Vに変更
if(V_l > 2.0){
V_l = 2.0;
}
出力が大きくなりすぎてモータやモータドライバが壊れないように、出力電圧に制限をかけています。
7.モータへ出力
//バッテリー電圧からデューティを計算
Duty_r = V_r/V_bat;
Duty_l = V_l/V_bat;
//モータにPWMを出力
if(run_mode != TEST_MODE){
MOT_OUT_R =(short)(240.0 * Duty_r);
MOT_OUT_L =(short)(240.0 * Duty_l);
}
最後に、計算した電圧を出力に反映します。出力電圧とバッテリー電圧に対する比に、240を掛けた値をPWMに設定します。
以上をまとめると、加速度accel [m/s^2]と最高速度max_speed [m/s]を指定すれば、随時目標速度tar_speed [m/s]が計算され、それに追従するようモータの電圧が制御されます。また、壁があればそれに沿って進むための目標角速度tar_ang_vel [rad/s]が計算され、それに追従するようモータの電圧が制御される、というプログラムになっています。
走行の設定
では、accelとmax_speedをどのように設定するのか調べるため、run.cの中身を見てみましょう。straight関数とturn関数が用意してありますので、straight関数に着目します。
1.グローバル変数に値を代入
I_tar_ang_vel = 0; I_ang_vel = 0; I_tar_speed = 0; I_speed = 0; //走行モードを直線にする run_mode = STRAIGHT_MODE; //壁制御を有効にする con_wall.enable = true; //目標距離をグローバル変数に代入する len_target = len; //目標速度を設定 end_speed = end_sp; //加速度を設定 accel = acc; //最高速度を設定 max_speed = max_sp;
積分値のリセットや、走行モードの設定など、制御に使用するグローバル変数の設定をしています。
2.モータ出力をON
MOT_POWER_ON;
モータ出力をONにすると、int_cmt0関数の制御がマウス本体に反映されます。
なおこれ以降は、直進の最後に停止する場合と停止しない場合に分かれていますが、とりあえず停止する場合を考えます。
3.加速、定速走行
//減速処理を始めるべき位置まで加速、定速区間を続行 while( ((len_target -10) - len_mouse) > 1000.0*((float)(tar_speed * tar_speed) - (float)(end_speed * end_speed))/(float)(2.0*accel));
細かい制御はint_cmt0の割り込みでされるので、straight関数では減速が開始されるまで待機するだけです。目標距離len_target [mm]と現在距離len_mouse [mm]の差が、減速に必要な距離より大きい間、whileでループします。（少し補正してlen_targetを-10しています。）ちなみに減速に必要な距離は、なつかしの等加速度運動の公式 v1^2 – v0^2 = 2*a*xから計算します。（v1がtar_speed、v0がend_speed、aがaccel、xが求めたい距離です。）
4.減速
//減速処理開始
accel = -acc; //減速するために加速度を負の値にする
while(len_mouse < len_target -1){ //停止したい距離の少し手前まで継続
//一定速度まで減速したら最低駆動トルクで走行
if(tar_speed <= MIN_SPEED){ //目標速度が最低速度になったら、加速度を0にする
accel = 0;
tar_speed = MIN_SPEED;
}
}
accel = 0;
tar_speed = 0;
//速度が0以下になるまで逆転する
while(speed >= 0.0);
加速・等速走行のwhileループから抜けたら、加速度accel [m/s^2]を反転して減速を開始します。減速も細かい制御はint_cmt0任せです。目標距離len_target [mm]が現在距離len_mouse [mm]より大きい間、whileでループします。（少し補正を入れてlen_targetを-1しています。）
ただし、目標距離に届く前に速度が0になってしまうと、いつまでたってもループから抜け出せなくなるので、最低速度 MIN_SPEED を下回らないように補正しています。
そして目標距離に達したら、今度は速度speed [m/s]が0になるまでループです。
5.終了処理
//加速度とトルク入力を0にする accel = 0; //現在距離を0にリセット len_mouse = 0;
以上で直進が終了します。
次回
自律走行で直進する流れがわかったので、次回は実装して走らせてみたいと思います。