くねくねしていて曲がりくねった捻くれコース 規則的でないから難しい! さらによく見れば何かに見える!?
正面
緑で囲まれた部分は光センサー。
アーム
オレンジの丸で囲まれた部分で缶を支える。
タッチセンサー
ピンクの丸で囲まれた部分はタッチセンサーに缶が当たり損ねないように補助を付けて、前方のどこに当たってもよくした。
#define arm_up OnFwd(OUT_C);Wait(13);Off(OUT_C); //アームアップ #define arm_down OnRev(OUT_C);Wait(16);Off(OUT_C); //アームダウン #define THRE 40 //閾値(光センサー1) #define THRE3 44 //閾値(光センサー3) #define go OnFwd(OUT_A+OUT_B); //前進 #define back OnRev(OUT_A+OUT_B); //後退 #define right(t) OnFwd(OUT_A);OnRev(OUT_B);Wait(100); //右に向く(tによって角度変化) #define left(t) OnFwd(OUT_B);OnRev(OUT_A);Wait(t); //左を向く(tによって角度変化) #define turn OnFwd(OUT_A);OnRev(OUT_B);Wait(200);Off(OUT_A+OUT_B); //180度回転 task main() { SetSensor(SENSOR_1,SENSOR_TOUCH);//1はタッチセンサ SetSensor(SENSOR_2,SENSOR_LIGHT);//2は光センサ SetSensor(SENSOR_3,SENSOR_LIGHT);//3は光センサ int flag=0; while (flag<6) { // if (SENSOR_2 > THRE) {if (SENSOR_3 > THRE3){go;} //光センサ2、3が白にある時 else{OnFwd(OUT_B);OnRev(OUT_A);} //光センサ3が黒線上にある時 } else {if (SENSOR_3 > THRE3){OnFwd(OUT_A);OnRev(OUT_B);} //光センサ2が黒線上にある時 else{if(flag==0){Off(OUT_A+OUT_B);Wait(100);go;Wait(150);turn(100);arm_up;back;Wait(60);right(100); flag++;} //最初の交差点に空き缶を置く else if(flag==2){Off(OUT_A+OUT_B);Wait(100);go;Wait(30);flag++;}//2回目の交差点 else if(flag==3){Off(OUT_A+OUT_B);Wait(100);go;Wait(150);turn(100);arm_up;back;Wait(60);right(100);flag++;}//三回目の交差点 else if(flag==4){Off(OUT_A+OUT_B);Wait(100);go;Wait(30);flag++;} else if(flag==5){Off(OUT_A+OUT_B);Wait(100);go;Wait(30);flag++;} } } if(SENSOR_1==1){if((flag==0)||(flag==3)){Off(OUT_A+OUT_B);Wait(100);arm_down;}//空き缶キャッチ else if(flag==1){Off(OUT_A+OUT_B);Wait(100);left(110);go;Wait(190);right(10); go;Wait(190);right(50);go;Wait(160);left(100);flag++;} //最初の交差点空き缶避ける } } Off(OUT_A+OUT_B); }
#define arm_up OnFwd(OUT_C);Wait(10); //アームを上げる #define arm_down OnRev(OUT_C);Wait(10); //アームを下げる #define sensor 40 //閾値 #define sensor2 44 #define go OnFwd(OUT_A+OUT_B); //前進 #define back OnRev(OUT_A+OUT_B); //後退 #define right OnFwd(OUT_A);OnRev(OUT_B);Wait(100);Off(OUT_A+OUT_C); //右に向く #define lefr OnFwd(OUT_B);OnRev(OUT_A);Wait(100);Off(OUT_A+OUT_C); //左に向く #define crossing_right(t) Off(OUT_A+OUT_B);Wait(100);OnFwd(OUT_B);OnRev(OUT_A);Wait(20);OnFwd(OUT_A+OUT_B);Wait(t);kousaten++; //交差点の対処 #define crossing_left(t) Off(OUT_A+OUT_B);Wait(100);OnFwd(OUT_A);OnRev(OUT_B);Wait(20);OnFwd(OUT_A+OUT_B);Wait(t);kousaten++; //交差点の対処 #define turn OnFwd(OUT_A);OnRev(OUT_B);Wait(200);Off(OUT_A+OUT_B); //180度回転
task main() { SetSensor(SENSOR_1,SENSOR_TOUCH);//1はタッチセンサ SetSensor(SENSOR_2,SENSOR_LIGHT);//2は光センサ SetSensor(SENSOR_3,SENSOR_LIGHT);//3は光センサ while (true) { start follow_line; //ライントレース開始 until(SENSOR_1==1); //タッチセンサーが反応したら以下のプログラムを実行 stop follow_line; //ライントレース停止 arm_down; OnRev(OUT_A+OUT_B); //缶を掴みつつ回転,缶を後方へ OnFwd(OUT_C);Wait(180); Off(OUT_B); back;Wait(40); //位置修正 arm_up; back;Wait(50); //缶から離れる turn; } }
task follow_line () //ライントレースのプログラム { int kousaten=0; while (kousaten<6) { if (SENSOR_2 > sensor) {if (SENSOR_3 > sensor2){go;} //光センサ2、3が白にある時前進 else{OnFwd(OUT_B);OnRev(OUT_A);} //光センサ3が黒線上にある時 } else {if (SENSOR_3 > sensor2){OnFwd(OUT_A); //光センサ2が黒線上にある時 OnRev(OUT_B);} else{if(kousaten==0){crossing_right(30);} //最初の交差点 else if(kousaten==1){crossing_left(40);} else if(kousaten==2){crossing_right(40);} else if(kousaten==3){crossing_left(30);} else if(kousaten==4){crossing_right(30);} else if(kousaten==5){crossing_left(30);} } } } Off(OUT_A+OUT_B); }