#define THRESHOLD 46 #define go(t) OnRev(OUT_AC);Wait(t);Off(OUT_AC); //t秒前進 #define back(t) OnFwd(OUT_AC);Wait(t);Off(OUT_AC); //t秒後進 #define roll_left OnFwd(OUT_C);OnRev(OUT_A); //左旋回 #define turn_left OnRev(OUT_A);Off(OUT_C); //左折 #define roll_right OnFwd(OUT_A);OnRev(OUT_C); //右旋回 #define turn_right OnRev(OUT_C);Off(OUT_A); //右折 #define roll_left2(t) OnFwd(OUT_C);OnRev(OUT_A);Wait(t); //t秒左旋回 #define turn_left2(t) OnRev(OUT_A);Off(OUT_C);Wait(t); //t秒左折 #define roll_right2(t) OnFwd(OUT_A);OnRev(OUT_C);Wait(t); //t秒右旋回 #define turn_right2(t) OnRev(OUT_C);Off(OUT_A);Wait(t); //t秒右折 #define roll_left3 OnRev(OUT_A);OnFwd(OUT_C);until(SENSOR_2<=43);Off(OUT_AC); //ラインを見つけるまで左折 #define roll_right3 OnRev(OUT_C);OnFwd(OUT_A);until(SENSOR_2<=43);Off(OUT_AC); //ラインを見つけるまで右折 #define close SetPower(OUT_B,1);OnFwd(OUT_B);Wait(50);Off(OUT_B);//アームを閉じる #define open SetPower(OUT_B,1);OnRev(OUT_B);Wait(70);Off(OUT_B);//アームを開ける #define cross_stop Off(OUT_AC);Wait(100); //交差点で1秒停止 #define STEP 2 sub LINE_R() //線の右側をトレース { SetSensor(SENSOR_2,SENSOR_LIGHT); ClearTimer(0); while(FastTimer(0)<=23)//0.23秒以内の場合はループ { if (SENSOR_2 > THRESHOLD +5) { roll_left; ClearTimer(0); }else if (SENSOR_2 > THRESHOLD +3){ turn_left; ClearTimer(0); }else if (SENSOR_2 > THRESHOLD){ OnRev(OUT_AC); ClearTimer(0); }else if (SENSOR_2 > THRESHOLD -3){ turn_right; ClearTimer(0); }else{ roll_right; } Wait(STEP); } Off(OUT_AC); } sub LINE_L() //線の左側をトレース { SetSensor(SENSOR_2,SENSOR_LIGHT); ClearTimer(0); while(FastTimer(0)<=21)//0.20秒以内の場合はループ { if (SENSOR_2 > THRESHOLD +5) { roll_right; ClearTimer(0); }else if (SENSOR_2 > THRESHOLD +3){ turn_right; ClearTimer(0); }else if (SENSOR_2 > THRESHOLD){ OnRev(OUT_AC); ClearTimer(0); }else if (SENSOR_2 > THRESHOLD -3){ turn_left; ClearTimer(0); }else{ roll_left; } Wait(STEP); } Off(OUT_AC); } sub CATCH() //コップをつかむ { roll_right2(70); go(30); close; roll_left2(80); go(30); roll_right3; } sub RELEASE() //コップを離す { roll_right2(90); open; turn_left2(30); back(100) turn_right2(120); } task main() { LINE_R();//A~B go(20); roll_left3; LINE_R();//B~C roll_right2(20); LINE_R();//C~F go(25); roll_left3; LINE_R();//F~第2ヘアピンカーブ go(25); LINE_L();//第2ヘアピンカーブ~R cross_stop;//R roll_left2(20); LINE_L();//R~P CATCH(); LINE_L();//P~Q go(30); LINE_R();//Q~S cross_stop;//S go(15 ); LINE_R();//S~S cross_stop;//S go(20); roll_left3; LINE_R();//S~F cross_stop;//F RELEASE(); LINE_R(); cross_stop;//C roll_right2(50);//Dへ go(120); }