課題の詳細は2017a/Mission2を参照。E地点で曲がるコースを選択した。
今回ロボットの長さが18cm以内という規定があったので、説明書の形から後輪とセンサーをより本体に近づけることで解決した。
また、後輪を近づけたことで後輪の向きによって本体の角度が変わりセンサーの感度が安定しないため、センサーが上下に自由に動くようにした。
白と黒を判断するためのセンサーの値のしきい値を定義
#define kijun 45
白い部分にはみ出た時に黒線まで戻るためのマクロを定義
#define turnl OnFwd(OUT_B,10);OnRev(OUT_C,20);until(SENSOR_3<kijun); #define turnr OnFwd(OUT_C,10);OnRev(OUT_B,20);until(SENSOR_3<kijun);
今回は電池の残量に影響を受けないように、タイヤの回転角で直進や右折左折を定義した。
#define turn(s,t,u) RotateMotorEx(OUT_BC,s,t,u,true,true);Off(OUT_BC);Wait(200);
右折、左折、直進、一時停止のマクロを定義
#define turnR turn(25,200,100);;turnl;Off(OUT_BC); #define turnL turn(25,200,-100);;turnr;Off(OUT_BC); #define tyokusin turn(10,100,0);Off(OUT_BC); #define teisi PlaySound(SOUND_DOWN);Off(OUT_BC);Wait(1000);
スタートの仕方を定義
#define hajimari OnFwd(OUT_BC,20);until(SENSOR_3<kijun);tyokusin;
場合によって黒線の右と左どちらでトレースするかを分ける必要があったので、ライントレースするための基本の2つのサブルーチンを定義した。
十字路や丁字路は黒色が続く秒数を計ることで判断した。
sub lineR() { long t0=CurrentTick(); while(CurrentTick()-t0<=300)//黒が0.3秒以上で次に進む { OnFwd(OUT_BC,30); if(SENSOR_3>kijun) { Off(OUT_BC); OnFwd(OUT_B,15); OnRev(OUT_C,8); Wait(1); t0=CurrentTick(); } if(SENSOR_3<kijun) { Off(OUT_BC); OnFwd(OUT_C,20); OnRev(OUT_B,10); Wait(1); } } } sub lineL() { long t0=CurrentTick(); while(CurrentTick()-t0<=300)//黒が0.3秒以上で次に進む { OnFwd(OUT_BC,30); if(SENSOR_3>kijun) { OnFwd(OUT_C,15); OnRev(OUT_B,8); Wait(1); t0=CurrentTick(); } if(SENSOR_3<kijun) { OnFwd(OUT_B,20); OnRev(OUT_C,10); Wait(1); } } }
直角に曲がる部分(H、G、F)では、その直前の動作から外側をトレースする必要があったため、白が続く秒数で判断する専用のサブルーチンを定義した。
sub line90() { long t0=CurrentTick(); while(CurrentTick()-t0<=300)//白が0.3秒以上で次に進む { OnFwd(OUT_BC,30); if(SENSOR_3>kijun) { OnFwd(OUT_C,20); OnRev(OUT_B,10); Wait(1); } if(SENSOR_3<kijun) { OnFwd(OUT_B,20); OnRev(OUT_C,10); Wait(1); t0=CurrentTick(); } } }
T地点の円では円半径が小さいので、なるべく小さく細かく曲がるように定義した。
sub curve()//通常より細かくターン { long t0=CurrentTick(); while(CurrentTick()-t0<=400)//黒が0.4秒以上で次に進む { OnFwd(OUT_BC,30); if(SENSOR_3>kijun) { OnFwd(OUT_C,20); OnFwd(OUT_B,10); Wait(1); t0=CurrentTick(); } if(SENSOR_3<kijun) { OnFwd(OUT_B,25); OnRev(OUT_C,25); Wait(1); } } }
HからGまでの小さいU字路が続く部分では、黒線の左側をトレースしているため内側を回る1つ目と、外側を回る2つ目で分けて定義した。
ここでは急カーブを判断して次の動作に進むよりも、時間で判断したほうがうまく動かせたので、サブルーチンではなくtaskを用いた。
task ujiro1() //通常よりも細かくターン { while(true) { OnFwd(OUT_BC,30); if(SENSOR_3>kijun) { OnFwd(OUT_C,20); OnFwd(OUT_B,5); Wait(1); } if(SENSOR_3<kijun) { OnFwd(OUT_B,25); OnRev(OUT_C,25); Wait(1); } } } task ujiro2() //通常よりも細かくターン { while(true) { OnFwd(OUT_BC,30); if(SENSOR_3>kijun) { OnFwd(OUT_C,25); OnRev(OUT_B,20); Wait(1); } if(SENSOR_3<kijun) { OnFwd(OUT_B,20); OnRev(OUT_C,10); Wait(1); } } }
最後にA地点にたどり着いたあとの動きのためのサブルーチンを定義した。
sub goal() { Off(OUT_BC); Wait(200); OnFwd(OUT_C,20); OnRev(OUT_B,10); Wait(300); Off(OUT_BC); Wait(200); turn(25,410,0);//枠内まで直進 Off(OUT_BC); PlaySound(SOUND_DOWN); }
基本的にはマクロとサブルーチンを順番に並べただけのものになっている。
task main() { SetSensorLight(S3); hajimari; turnl; lineR();//ライントレース開始 PlaySound(SOUND_UP);//E地点 Off(OUT_BC); turnR; lineR(); teisi;//P地点 turnL; lineL();
丁字路、十字路の判断時に生まれる誤差を修正するためのターンを入れている。
PlaySound(SOUND_UP);//Q地点 turn(20,120,100);//軌道修正 tyokusin; lineL(); PlaySound(SOUND_UP);//R地点 tyokusin; turnL; lineL(); teisi;//T地点 turn(20,30,100);//軌道修正 tyokusin; turnr; curve(); teisi;//T地点 turn(20,60,100);//軌道修正 tyokusin; turnr; line90();
U字路のtaskの切り替えは適当な時間で指定した。
PlaySound(SOUND_UP);//H地点 turnr; lineL(); start ujiro1;//急カーブだと判断したらU字路スタート Wait(10000); stop ujiro1;//U字路が完全に終了する時間でストップ start ujiro2;//曲がり切って数秒で次のU字路スタート Wait(15000); stop ujiro2;//U字路が完全に終了する時間でストップ line90();//曲がり切って数秒で直角用のライントレース開始 PlaySound(SOUND_UP);//G地点 turnr; lineL(); teisi;//S地点 turnL; lineL(); PlaySound(SOUND_UP);//P地点 turn(20,120,100);//軌道修正 tyokusin; lineL(); PlaySound(SOUND_UP);//Q地点 tyokusin; turnL; line90(); PlaySound(SOUND_UP);//F地点 turnr; lineL(); start ujiro1;//急カーブだと判断したらU字路スタート Wait(15000); stop ujiro1; lineL(); tyokusin;//E地点 lineL(); goal(); }
直線でのスピードがかなり遅く、スタートからゴールまで4分半もかかってしまう。
今回は急カーブなどで曲がり方を分けて設定すること、タイヤの回転角で動作を指定することで、 しっかりと安定してゴールすることは可能になった。
しかし、閾値の設定が一つであったため直線でまったくスピードが出なかった。 より細かく閾値を設定することが必要であった。