コースは第二コースを選定した。
急カーブのコースにも対応させるべく、小回りが利くように左右のタイヤ間隔は狭めに制作した。また、前回の書道ロボット制作時、想像以上に電池の交換回数が多く、そのたびにロボットを分解しなければならなかったので、今回は分解しなくても電池を交換できる構造とした。 缶をつかむ方式は、多少位置がずれても確実に缶をつかめるよう、2本の腕をギアを用いて連動させ、クワガタムシのように左右から挟む構造とした。また、腕を長めに作り、コースの円のふちに止まったまま缶をつかめるようにした。缶をつかむ部分の動力の取り付けには苦労した。 センサーはできる限り地面に近くなるように設置した。
#define base 52 //白黒判定の基準値 #define SPEEDS 50 #define SPEEDCl 30 #define SPEEDCH 50 #define go_forward OnFwd(OUT_BC,SPEEDS); // 直進 #define turn_left OnFwd(OUT_B,SPEEDCl); OnRev(OUT_C,SPEEDCl); #define turn_right OnFwd(OUT_C,SPEEDCl); OnRev(OUT_B,SPEEDCl); #define STEP 2 // 一回の判定で進む時間 #define nMAX 110 #define short_break Off(OUT_BC); Wait(2000); #define MOVE_TIME 7500 #define CROSS_TIME 300 // 交差点通過にかかる時間 #define CROSS_TIME2 1600 //方向転換に要する時間 #define release RotateMotor(OUT_A, 30, 40);//缶を開放 #define catch RotateMotor(OUT_A, -30, 40); //缶をつかむ #define crossline OnFwd(OUT_BC,SPEEDS);Wait(CROSS_TIME);short_break; //交差点を通過 #define crossline_left crossline; OnFwd(OUT_B,SPEEDCl); OnRev(OUT_C,SPEEDCl);Wait (CROSS_TIME2);short_break; //交差点を左折 #define crossline_right crossline; OnFwd(OUT_C,SPEEDCl); OnRev(OUT_B,SPEEDCl);Wait(CROSS_TIME2);short_break; //交差点を右折
左右片方のタイヤのみを動かして曲がるのは動作や軌道が安定しないので、曲がるときはつねに左右のタイヤを逆方向に動かすようにした。
缶を掴んだり開放したりする動作は、時間ではなく角度で指定することで、プログラムしやすくした。
左折右折は、当初以下のように定義していた
#define crossline_left OnFwd(OUT_B,SPEEDCl); OnRev(OUT_C,SPEEDCl);Wait (CROSS_TIME2);short_break; //交差点を左折 #define crossline_right OnFwd(OUT_C,SPEEDCl); OnRev(OUT_B,SPEEDCl);Wait(CROSS_TIME2);short_break; //交差点を右折
しかしこのようにすると、交差点判断前後のセンサーの軌道は下図のようになる
このままでは線のわきに戻るのに時間がかかるうえ、線に直角に進入してしまい、交差点と誤認してしまうことがあった。 そこで、交差点を認識した後いったん線をまたぎ、そこから左右に旋回するようプログラムした。
こうすることで確実に線上に戻ることができるようになった。
sub crossjudge_right() { int nOnline=0; // 続けて黒になった回数 (カウンタ) while (nOnline < nMAX) { if (SENSOR_2 < base-10) { // 線上なら turn_right; // 右へ nOnline++; // カウンタを増やす } else { if (SENSOR_2 < base) { // 境界付近なら go_forward; // 直進 } else { // 線からはずれれば turn_left; // 左へ } nOnline=0; } Wait(STEP); } } sub crossjudge_left() { int nOnline=0; // 続けて黒になった回数 (カウンタ) while (nOnline < nMAX) { if (SENSOR_2 < base-10) { // 線上なら turn_left; // 左へ nOnline++; // カウンタを増やす } else { if (SENSOR_2 < base) { // 境界付近なら go_forward; // 直進 } else { // 線からはずれれば turn_right; // 右へ } nOnline=0; } Wait(STEP); } }
ライントレースのかなめとなる部分である。連続して黒と判定した回数をカウントし、一定数(nMAX)に達したら交差点だと判断するプログラムである。 線の右側左側のどっちをたどるかで2種類のサブルーチンを用意した。 センサーと地面をかなり近づけられたので、3段階の判定で十分に走行させることができた。
task main() { SetSensorLight(S2); crossjudge_right(); crossline_right; //Bを右折 crossjudge_right(); short_break; //J到着 catch; short_break; crossline_left; repeat(3) { crossjudge_left(); crossline; } //KとLを通過 crossjudge_left(); crossline_left; //Iを左折 short_break; crossjudge_left(); OnFwd(OUT_BC,SPEEDS); Wait(100);Off(OUT_BC); crossline_left; //Hをラインまたいで左折 crossjudge_right(); crossline; crossjudge_left(); crossline; crossjudge_right(); short_break; crossline_right; //Gで右折 crossjudge_right(); crossline_right; //Fを右折 crossjudge_right(); crossline; //E通過 crossjudge_right(); //D到着 OnFwd(OUT_B,SPEEDCl); OnRev(OUT_C,SPEEDCl);Wait(1500);short_break; OnRev(OUT_BC,SPEEDS); Wait(500);Off(OUT_BC); release; short_break; OnRev(OUT_BC,SPEEDS); Wait(500);Off(OUT_BC); catch; crossjudge_right(); crossjudge_right(); crossline; //Cを通過 crossjudge_right(); Off(OUT_ABC); //A到着 }
プログラムを組むにあたって、以下の点で工夫した。