こんにちわ、青木です。
HM-StartKit初級者編も最後になってしまいました。
探索中の走行を見ていると前壁があったら曲がることが多いですが、迷路によっては曲がる区画で前壁がないときがあります。その場合、前回のように前壁センサを使って位置補正をすることができません。今回は、それを解決する方法として横壁センサを使った位置補正を紹介します。
横壁センサを使って位置補正の原理
マイクロマウスは壁との距離を保って迷路の中央にいるように制御しています。うまく制御できていると横壁との距離は一定であり、横壁センサの値も一定ということになります。ここまでうまく制御できていたら横壁のセンサを使って位置補正をする必要がないのではと考える方もいると思いますが、全国大会のマイクロマウス(旧ハーフサイズ)競技では32×32区画というかなり大きなフィールドを走破する必要があり、迷路によっては30区画長い直線の後、前壁なしで曲がるということも考えられます。そんな時、横壁センサでほんの少し位置補正ができればなんとかなったのに〜ということが考えられます。
横壁センサの値を観察すると、壁がある時は、壁との距離を一定になるように制御しているので、センサの値はほぼ同じですが、壁があるところからないところに移る時センサの値はどのように変化するでしょうか?壁との距離を一定になっている時のセンサの値が突然0になるのでしょうか?姿勢制御せずに(ゲイン0)、一区画(90mm)走行したとき横壁センサの値をグラフ化したものが次の図になります。
センサの値が急激に下がっていることがわかります。探索中、壁の有無を正しく認識している場合は、おそらく壁の有無の閾値が、センサの値が下がり終わったぐらいのところになっていると思います。もし、センサの値が急激に下がっている最中のところに閾値があるとすれば、おそらく探索中に壁の有無を間違っている可能性があります。
迷路の中央を走っているとき、壁ありから壁なしとなる位置は、大体同じ位置になることが多いです。HM-StaterKitなら、走行した距離はlen_mouseの変数に入っています。壁切れ補正確認用の関数を新しく作って走行し、壁が無くなったと判断したlen_mouseの値を別の変数に保存しておきます。探索走行時、壁が無くなった距離と、先程取得した距離の差分が絶対値で10より大きい場合は、それは、誤動作の可能性があるので、その時は、無視します。絶対値で10より小さい場合は、会場での明るさのばらつきを考慮して壁が無くなった距離と、先程取得した距離の平均としてlen_mouseに戻してあげると良いでしょう。
このように横壁で位置補正することをマイクロマウスの業界では壁切れ補正と呼んでいます。
壁切れ補正確認用の関数
加減速中に補正をすると、予定の速度まで減速できず次の行動をするので、タイヤが滑ってうまく位置補正ができないことがあります。ここでは探索の90mm走行時の安定したところで位置補正をすることにします。前回のstraight関数にフラグをつけて機能(壁切れ補正の位置確認)を追加しても良いのですが、90mm加減速せずに走行する関数は難しくないので、新しくchech_straight関数を作ります。基になった関数は、straight関数です。加減速のところを削除するとシンプルな構成になります。違いは、90mm走行中に壁の切れ目(壁ありから壁なしになったところ)の判断を17から22行目に追加しています。ここで取得しているのは壁ありから壁なしの時のみです。その逆のパターンも必要になったら追加してください。壁ありから壁なしのパターンだけの補正だけでもかなり有効です。
少しプログラムの解説をするとsen_r.is_wall==falseだけだと壁がない時はif文がtrueになってしまうため、壁がない間r_adjust_lenが更新されてしまい、壁ありから壁なしになった時の距離を見ることができません。壁ありから壁なしに変化した時だけr_adjust_lenを更新する必要があります。r_adjust_lenを、更新したか、更新していないかのフラグが必要ということになります。flagという変数を用意して更新の管理をするのも良いのですが、ここでは、r_adjust_lenの変数を使って更新管理をしています。この関数では、r_adjust_lenの初期値を0としています。値が更新されると0ではない値が入るためフラグとして使用することができます。r_adjust_lenをフラグとして使用しているのでr_adjust_len==0という条件を追加しています。
run.c
float r_adjust_len,l_adjust_len;
void check_straight(float end_sp)
{
I_tar_ang_vel = 0;
I_ang_vel = 0;
I_tar_speed = 0;
I_speed = 0;
len_mouse = 0;
run_mode = STRAIGHT_MODE;
con_wall.enable = false;
end_speed = end_sp;
max_speed = end_sp;
access = 0;
MOT_POWER_ON;
r_adjust_len = l_adjust_len = 0;
while(len_mouse < 90.0){
if( (sen_r.is_wall==false) && (r_adjust_len==0)){
r_adjust_len = len_mouse;
}
if( (sen_l.is_wall==false) && (l_adjust_len==0)){
l_adjust_len = len_mouse;
}
}
len_mouse = 0;
}
void get_adjust_len(float * r_len, float * l_len){
*r_len = r_adjust_len;
*l_len = l_adjust_len;
}
このままでは、r_adjust_lenとl_adjust_lenが正しく取得できたかどうかが操作者ははわからない状態です。そこで、r_adjust_lenとl_adjust_lenはrun.cのグローバル変数で宣言してありますが、違うファイルからアクセスするにはexternが必要になるので、簡単に値を確認できるようにget_adjust_lenという関数を作りました。引数にポインタを使っているのでget_adjust_lenを呼びしたファイル内で宣言した変数に値をコピーするようになっています。
データの確認
adjust.cのファイルから先程取得した補正値の値をモード15の調整モードで確認してみます。モード15の調整モードはadjust.cのadjust関数となります。調整モードの中のcase 4、case 5はまだ使われてないので、case 4とcase 5に補正値を走行して取得するコードと値を確認するコードを追加します。追加したコードは以下になります。
case 4:
if(sen_fr.value + sen_fl.value + sen_r.value + sen_l.value > SEN_DECISION *4 ){
BEEP();
wait_ms(500);
straight(HALF_SECTION, SEARCH_ACCEL, SEARCH_SPEED, SEARCH_SPEED);
check_straight(SEARCH_SPEED);
straight(HALF_SECTION, SEARCH_ACCEL, SEARCH_SPEED, 0);
}
break;
case 5:
if(sen_fr.value + sen_fl.value + sen_r.value + sen_l.value > SEN_DECISION *4 ){
BEEP();
wait_ms(500);
get_adjust_len(&r_len,&l_len);
SCI_printf("r adjust len=%f , l adjust len=%f¥n¥r",r_len, l_len);
}
break;
r_lenとl_lenの変数は、adjust関数の上の方でfloat r_len,l_len;で宣言してあります。また、check_straightとget_adjust_lenの関数を使えるようにrun.hにプロトタイプ宣言しています。
SCI_printfの改良
r_lenとl_lenの型はfloatで宣言しています。HM-StaterKitのサンプルにあるSCI_printfは、マイコン内のメモリをあまり消費したくないため簡易printfとして提供しています。この簡易SCI_printfは、浮動小数点を表示できない仕様になっています。%fを使って補正の値を確認することができないので、intにキャストする必要があります。しかしこれでは小数点以下を確認することができません。1mm以下は誤差範囲となりますが、 少数点第一位ぐらいまでみたいと思いませんか?そこで、SCI_printf関数の中身を書き換えます。できるかぎりメモリの消費を抑えたいので、バッファーオーバーという危険な仕様がありますが、vsprintfを使うことにしました。vsprintfは、printfの引数を文字列に変換する関数で、
short SCI_printf(char *string, ...){
char buf[100];
va_list ap;
char length=0,i;
va_start(ap,string);
length=vsprintf(buf,string,ap);
va_end(ap);
for(i=0;i<length;i++){
SCI_putc(buf[i]);
}
}
vsprintfの関数を使うと、printfと同じフォーマット指定子を使うことができます。これで浮動小数点も表示することができるということになります。
実際取得した値は、
でした。
距離補正の確認
補正のコードは、straight関数に追加します。補正用の距離を取得した時は、壁ありから壁なしのパターンのみ取得しています。それ以外のパターンでは補正用の距離を取得していないので、壁ありから壁なしになるときだけ補正が掛かるようにします。
追加したコードを以下に示します。
#define r_hosei 52.847756
#define l_hosei 52.559981
void straight(float len,float acc, float max_sp,float end_sp){
char r_wall_check=0, l_wall_check=0, hosei_f=0;
I_tar_ang_vel = 0;
....
//モータ出力をON
MOT_POWER_ON;
if((end_speed!=0) && (len==SECTION)){
r_wall_check = sen_r.is_wall;
l_wall_check = sen_l.is_wall;
}
if(end_speed == 0) { //最終的に停止する場合
....
while(((len_target-10) -len_mouse) > 1000.0*((float)(tar_speed * tar_speed) - (float)(end_speed * end_speed))/(float)(2.0*accel))){
if(len==SECTION){
if((sen_r.is_wall==false) && (r_wall_check==true) && (hosei_f==0)){
len_mouse = (len_mouse+r_hosei)/2;
hosei_f=1;
}
if((sen_l.is_wall==false) && (l_wall_check==true) && (hosei_f==0)){
len_mouse = (len_mouse+l_hosei)/2;
hosei_f=1;
}
}
}
accel = -acc;
while(len_mouse < (len_target+5)){
....
51行目で、90mm走行の走行を開始する直前に壁の情報を取得しています。ここで壁がないと判断した時は補正の対象外となります。75行目から走行距離の指定が90mmの時にみ補正するようになっています。これがないと半区画(探索時の加速と減速区画)と2区画以上(最短走行時)走行するときも補正をしてしまうためです。2区画以上の場合は、走行終了の1区画前から壁なしを検出できるように工夫し、先程取得した距離を走行区画に換算すれば補正をすることができます。ここではシンプルに探索の時のみとします。77行目では、補正の距離と実際のソフトウェア上の距離の平均として走行距離にフィードバックしています。これは、壁が切れた位置が必ずしも正しい位置とは限らないので、半分半分信じるようにして平均化という手段をとっています。壁が切れた位置の信頼性が高い場合は、len_mouse*0.2+r_hosei*0.8のようなフィルターにすると良いでしょう。この時の壁が切れた位置の信頼性は80%としています。係数は足して”1″にしないと予定の距離と異なってしまいます。
この補正を追加するとスタート位置を後ろにおいても壁の切れ目で補正がかかり、ほぼ区画の中央で停止しているが確認できます。完全に区画の中央で停止していないのは、補正量を平均値としているからです。
お知らせ
手のひらサイズのマイクロマウスキット「HM-StarterKit」
33％OFFの特別価格で販売中！
製品ページはこちら
アールティロボットショップで購入する
