2019a/Member
課題2
目次
ルート:
A地点から出発 → B → C(直進) → D(一時停止の後、直進) → E → F → G(一時停止の後、右折) → H → I → J(右折) → K(左折) → L(ピンポン玉をつかむ) → K(直進) → M(一時停止) → シュート→ A地点に入る(ゴール)
トレースの仕組みは、白と黒の境界の値を測って、
その値より小さいところと、大きいところを2段階で分けて、
プログラムを書きました。
ピンポン玉の掴み方は、二つのブロックで挟むようにしました。
シュートの仕方は、シュートするところまで持って行って、
ピンポン玉を開放して、ロボット本体で押し出すようにしました。
ロボット本体は、横についた前輪2つで動いています。
前輪は重心のやや前についているので、ロボットを支えるための
後輪を1つ付けました。
光センサーは、タイヤとピンポン玉をつかむ部分の間につけた。
ロボット本体に対して真ん中につけることで、ロボット本体がラインの中央をトレースするようにした。
ピンポン玉をつかむ部分は、上にモーターを取り付け、
回転方向を90度変換させて、下の歯車に対応させました。
開きすぎた場合は、白い歯車が空回りするようになっています。
シュートの仕方は、ピンポン玉を開放してから、ロボット本体で
押し出すようにしました。
#define Threshold 44 #define front OnFwd(OUT_AC); #define left OnRev(OUT_A);OnFwd(OUT_C); #define right OnRev(OUT_C);OnFwd(OUT_A); //ラインをトレースするサブルーチン sub follow_line(t){ SetSensor(SENSOR_2, SENSOR_LIGHT); ClearTimer(0); while(FastTimer(0) < t){ if(SENSOR_2 < Threshold - 4){ //境界より4以上黒に近い時 right; }else if(SENSOR_2 < Threshold - 2){ //境界より2以上黒に近い時 Off(OUT_A);OnFwd(OUT_C); ClearTimer(0); }else if(SENSOR_2 < Threshold){ //境界から黒に2までの範囲 front; ClearTimer(0); }else if(SENSOR_2 < Threshold + 2){ //境界から白に2までの範囲 Off(OUT_C);OnFwd(OUT_A); ClearTimer(0); }else{ //境界より2以上白に近い時 left; ClearTimer(0); } Off(OUT_AC); } //交差点を直進するサブルーチン sub cross_line(){ SetSensor(SENSOR_2, SENSOR_LIGHT); while(SENSOR_2 < Threshold){ front; } Off(OUT_AC); Wait(30); while(SENSOR_2 > Threshold){ left; } Off(OUT_AC); Wait(30); } //交差点を右折するサブルーチン sub cross_right(){ SetSensor(SENSOR_2, SENSOR_LIGHT); while(SENSOR_2 < Threshold + 2){ right; } Off(OUT_AC); Wait(30); } //交差点を左折するサブルーチン sub cross_left() { SetSensor(SENSOR_2, SENSOR_LIGHT); while(SENSOR_2 < Threshold){ front; } Off(OUT_AC); Wait(30); while(SENSOR_2 > Threshold) { left; } Off(OUT_AC);Wait(30); } //ピンポン玉をつかむサブルーチン sub pinpon_catch(){ OnRev(OUT_B);Wait(30); //アームを閉じる Off(OUT_B);Wait(30); } //ピンポン玉をシュートするサブルーチン sub pinpon_shoot(){ OnFwd(OUT_B);Wait(20); //アームを開く Off(OUT_B);Wait(100); front;Wait(20); //ピンポン玉を押し出す Off(OUT_AC); }
OnFwd(OUT_AC);Wait(80); //AからC Off(OUT_AC);Wait(30); follow_line(20); cross_line(); //CからD follow_line(20); cross_line(); //DからE follow_line(20); cross_right(); //EからF follow_line(100);follow_line(20); cross_right(); //FからG follow_line(50);follow_line(15); cross_right(); // GからH follow_line(90);follow_line(15); cross_line(); // HからI follow_line(90);follow_line(15); cross_line(); // IからJ follow_line(90);follow_line(15); cross_right(); // JからK follow_line(68);follow_line(15); cross_left(); //KからL follow_line(130); Wait(30); pinpon_catch(); //ピンポン玉を掴む
follow_line(19); //LからK cross_line(); //KからM follow_line(830);follow_line(17); OnFwd(OUT_C);Wait(20); Off(OUT_C);Wait(100); pinpon_shoot(); //シュート
連続して黒になる時間はサブルーチンのfollow_line(t)が
計っています。
これは交差点の判断に使います。
sub follow_line(t){ SetSensor(SENSOR_2, SENSOR_LIGHT); ClearTimer(0); //1番 while(FastTimer(0) < t){ //4番 if(SENSOR_2 < Threshold - 4){ //境界より4以上黒に近い時 right; }else if(SENSOR_2 < Threshold - 2){ //境界より2以上黒に近い時 Off(OUT_A);OnFwd(OUT_C); ClearTimer(0); //2番 }else if(SENSOR_2 < Threshold){ //境界から黒に2までの範囲 front; ClearTimer(0); //2番 }else if(SENSOR_2 < Threshold + 2){ //境界から白に2までの範囲 Off(OUT_C);OnFwd(OUT_A); ClearTimer(0); //2番 }else{ //境界より2以上白に近い時 left; ClearTimer(0); //2番 } Off(OUT_AC); //4番 }
#define Threshold 46 #define front OnFwd(OUT_AC); #define left OnRev(OUT_A);OnFwd(OUT_C); #define right OnRev(OUT_C);OnFwd(OUT_A); //ラインをトレースするサブルーチン sub follow_line(t){ SetSensor(SENSOR_2, SENSOR_LIGHT); ClearTimer(0); while(FastTimer(0) < t){ if(SENSOR_2 < Threshold - 4){ //境界より4以上黒に近い時 right; }else if(SENSOR_2 < Threshold - 2){ //境界より2以上黒に近い時 Off(OUT_A);OnFwd(OUT_C); ClearTimer(0); }else if(SENSOR_2 < Threshold){ //境界から黒に2までの範囲 front; ClearTimer(0); }else if(SENSOR_2 < Threshold + 2){ //境界から白に2までの範囲 Off(OUT_C);OnFwd(OUT_A); ClearTimer(0); }else{ //境界より2以上白に近い時 left; ClearTimer(0); } Off(OUT_AC); } //交差点を直進するサブルーチン sub cross_line(){ SetSensor(SENSOR_2, SENSOR_LIGHT); while(SENSOR_2 < Threshold){ front; } Off(OUT_AC); Wait(30); while(SENSOR_2 > Threshold){ left; } Off(OUT_AC); Wait(30); } //交差点を右折するサブルーチン sub cross_right(){ SetSensor(SENSOR_2, SENSOR_LIGHT); while(SENSOR_2 < Threshold + 2){ right; } Off(OUT_AC); Wait(30); } //交差点を左折するサブルーチン sub cross_left() { SetSensor(SENSOR_2, SENSOR_LIGHT); while(SENSOR_2 < Threshold){ front; } Off(OUT_AC); Wait(30); while(SENSOR_2 > Threshold) { left; } Off(OUT_AC);Wait(30); } //ピンポン玉をつかむサブルーチン sub pinpon_catch(){ OnRev(OUT_B);Wait(30); //アームを閉じる Off(OUT_B);Wait(30); } //ピンポン玉をシュートするサブルーチン sub pinpon_shoot(){ OnFwd(OUT_B);Wait(20); //アームを開く Off(OUT_B);Wait(100); front;Wait(20); //ピンポン玉を押し出す Off(OUT_AC); } task main() { OnFwd(OUT_AC);Wait(80); //AからC Off(OUT_AC);Wait(30); follow_line(20); cross_line(); //CからD follow_line(20); cross_line(); //DからE follow_line(20); cross_right(); //EからF follow_line(100);follow_line(20); cross_right(); //FからG follow_line(50);follow_line(15); cross_right(); // GからH follow_line(90);follow_line(15); cross_line(); // HからI follow_line(90);follow_line(15); cross_line(); // IからJ follow_line(90);follow_line(15); cross_right(); // JからK follow_line(68);follow_line(15); cross_left(); //KからL follow_line(130); Wait(30); pinpon_catch(); //ピンポン玉を掴む follow_line(19); //LからK cross_line(); //KからM follow_line(830);follow_line(17); OnFwd(OUT_C);Wait(20); Off(OUT_C);Wait(100); pinpon_shoot(); //シュート }
最初は、ロボットもプログラムも複雑になることを
想定していたけど、不必要な部分を見つけて、削っていったら、
ロボットもプログラムも小さくできたので良かった。
回転する時間を調整するのが難しかった。