こんにちわ、青木です。
前回の方法で壁の有無を判断する閾値を調整をすることで完走率が上がりますが、走行している間に姿勢制御やスリップなどで予定していた前壁との距離がずれてしまい、袋小路などで旋回ができなくなってリタイヤとなることがあります。ここでは、そのズレを補正する方法を紹介します。
車体位置補正
ここで紹介するのは、マイクロマウスの競技をみていると初級者がよくやっている方法です。超信地旋回をする区画で、壁に向かってマイクロマウスを当てて壁と垂直なるように姿勢を戻し、迷路の中央の位置までまっすぐ進むことで角度と位置を補正する方法です。文字としてわかりづらいので、下記の動作をみていただくと、あれかと思うはずですw。
超信地旋回を5回行ったら車体位置補正をする、または袋小路に入ったら車体位置補正をするという感じで行われています。両方行う方もいますが、ここでは袋小路に入って壁がある時に車体位置補正をする方法を説明します。
袋小路の判断
探索走行の動作は、右に曲がる、直進する、左に曲がる、180度旋回するの4つです。ここでは、180度旋回するところに車体位置補正のアルゴリズムを追加します。180度旋回する時は、袋小路(行き止まり)の時に180度旋回するケースと足立法でそのまま進むと遠いと判断した時に180度旋回する2つのケースあります。ここでは、180度旋回する場合を袋小路と表現しています。
180度旋回を指示しているところは、search.cのsearch_adachiの関数です。408行目にも同じコードがありますが、408行目は、ゴール区画の時に発動する可能性がありますが、今回は除外しています。
case rear:
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,0); //半区画進む
turn(180,TURN_ACCEL,TURN_SPEED,RIGHT); //180ターン
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,SEARCH_SPEED);
break;
姿勢を戻す壁の判断
180度旋回をするところでは、左、右、前壁があることが多いので、縦方向と横方向の位置と姿勢を戻すことができます。効率を考えたら、初めに横方向の位置と姿勢を直した後、縦方向の位置と姿勢を直した方が旋回の回数と誤差の蓄積が小さくなります。
初めに横方向から位置調整をします。右と左ともに壁がある場合は、車体が壁に当たらない方向で旋回します。その理由として、HM-Starterkitは車軸より前が31mm、車軸より後ろが22mmの基板サイズとなっているので、HM-StarterKitが左壁によっていた場合は、右に旋回した方が壁にぶつかる率が小さいからです。
ここでは、左壁を使って位置補正をする方法を紹介します。まず、右に旋回し、20mm以上後ろにバックします。30mmぐらいバックするように指示をすると壁にHM-StarterKitがほぼ確実に当たり、壁とHM-StarterKitが直角になります。その後、20mm前に進みます。20mm進むとHM-Starterkitが迷路の中央の位置にいることになります。これで横方向の位置補正が完了です。
次に縦方向の位置を調整します。上記の図の状態から右に旋回します。旋回した後、30mmバックして再び20mm前に進みます。以上のことをすると縦と横の方向の位置と姿勢を直すことができます。周囲3方面の壁に囲われていない場合、後ろに壁がないことがあるので、その時は右に旋回して終わりとなります。
プログラミング
ここまで説明から必要な関数は、30mmバックする関数と20mm前進する関数が必要であることがわかります。20mm前進する関数においては、探索で使用している直進の関数の引数を20mmにすれば良いだけなのですが、問題は、素直にバックできるかどうかです。現状のHM-StarterKitで使われている走行の関数を調べてからどのように追加するか考えてみます。
HM-StarterKitの走行の関数は、straight関数とturn関数の2種類です。関数の中を見ると直接モータドライバの回転方向を指示しているところがないため、別の関数でモータドライバに回転方向の指示をしているところがあるはずです。調査するとinterrupt.cの中でv_rとv_lの変数の正負をみて回転方法を決めていました。
if(run_mode != TEST_MODE){
//右モータの出力電圧が正の場合
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; //電圧を正の値へ反転
}
}
つまり、ターゲット速度をマイナスにするとバックする構造になっています。
次に、走行距離に関するところを調査します。エンコーダーの値を取得しているint_cmt2の関数を見ると、回転方向の指示を考慮せず、前回の角度と今の角度の差分を足しているだけなので、バックすると距離はマイナスになります。
diff_pulse_r=(locate_r - before_locate_r); ...
speed_new_r = (float)((float)diff_pulse_r * (float)MMPP); ...
len_mouse += (speed_new_r L speed_new_l)/2.0;
元々あったstraight関数の引数にマイナスの速度、マイナスの距離を入れて動作するか確認してみましたが、やはり、比較演算のところで不具合を起こしてしまうので、バックする関数を追加します。基本はstraight関数と同じなので、straight関数をコピーして必要なところだけ残し、符号を書き換えた形になります。コメントがあるところが変更箇所です。また、壁に向かってバックしますが、壁に当たってタイヤがロックした時、予定の30mm進むことができず、while文から抜け出せずその場に止まってしまうので、whileを実行する時間を1秒と時間制限を設けています。
extern volatile unsigned int timer;
void back(float len){
unsigned int start_timer;//タイヤがロックした時の対策
I_tar_ang_vel = 0;
I_ang_vel = 0;
I_tar_speed = 0;
len_mouse = 0;//距離の初期化
run_mode = STRAIGHT_MODE;
con_wall.enable = false;//壁制御を無効にする
len_target = len;//lenの値はマイナスが入っている
end_speed = 0;
accel = -1.0;//速度をマイナスにするとバックする
max_speed = 0.1
MOT_POWER_ON;
while(((len_target -10) -len_mouse) > 1000.0*((float)(tar_speed*tar_speed)-(float)(end_speed*end_speed))/(float)(2.0*accel));
access = 1.0;//マイナスの速度を0にするためプラスにしている
start_time = timer;
while(len_mouse > (len_target + 1)){
if(tar_speed >= -1*MIN_SPEED){//MIN_SPEEDはプラスの値なので、-1を掛けてマイナスにする
accel=0;
tar_speed = -1*MIN_SPEED;
}
if((timer - start_timer) > 1000 ){//１秒経ってもwhileを抜け出せない時の処置
break;
}
}
accel = 0;
tar_speed = 0;
len_mouse = 0;
}
袋小路に追加
ここでは、袋小路に入る時に、右の壁があったら左に旋回、右に壁がなく、左に壁があったら右に旋回、左右壁がなく、前に壁があったら180度旋回するといったフローにします。組み込むところは、search.cのsearch_adachiの関数の466行目あたりところです。
case rear:
char back_flag = sen_fr.is_wall | sen_fl.is_wall; //前壁の有無を調査
if(sen_r.is_wall == true){
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,0);//半区画進む
turn(90,TURN_ACCEL,TURN_SPEED,LEFT); //左に曲がって
back(-30); //30mm back
wait_ms(500);
straight(20,1,0.1,0); //20mm 前進
wait_ms(500);
if(back_flag == true){
turn(90,TURN_ACCEL,TURN_SPEED,LEFT); //左に曲がって
back(-30); //30mm back
wait_ms(500);
straight(20,1,0.1,0); //20mm 前進
wait_ms(500);
}else{
turn(90,TURN_ACCEL,TURN_SPEED,LEFT); //左に曲がって
}
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,SEARCH_SPEED);
}else if(sen_l.is_wall == true){
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,0);//半区画進む
turn(90,TURN_ACCEL,TURN_SPEED,RIGHT); //左に曲がって
back(-30); //30mm back
wait_ms(500);
straight(20,1,0.1,0); //20mm 前進
wait_ms(500);
if(back_flag == true){
turn(90,TURN_ACCEL,TURN_SPEED,RIGHT); //右に曲がって
back(-30); //30mm back
wait_ms(500);
straight(20,1,0.1,0); //20mm 前進
wait_ms(500);
}else{
turn(90,TURN_ACCEL,TURN_SPEED,RIGHT); //右に曲がって
}
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,SEARCH_SPEED);
}else{
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,0);//半区画進む
turn(180,TURN_ACCEL,TURN_SPEED,RIGHT); //180ターン
if(back_flag == true){
back(-30); //30mm back
wait_ms(500);
straight(20,1,0.1,0); //20mm 前進
wait_ms(500);
}
straight(HALF_SECTION,SEARCH_ACCEL,SEARCH_SPEED,SEARCH_SPEED);
}
break;
これを組み込むことで完走率が上がりますが、この補正を頼りにしてPIDの調整やセンサのリファレンスの取得などを疎かにすると完走率が下がってしまうので、調整をした後に補助的に追加する感じで使用すると良いでしょう。
次回は、壁距離センサと前壁を使って車体位置補正をする方法です。
お知らせ
手のひらサイズのマイクロマウスキット「HM-StarterKit」
33％OFFの特別価格で販売中！
製品ページはこちら
アールティロボットショップで購入する
