#define THRESHOLD 46
#define go_forward ; OnRev(OUT_AC);
#define turn_right1 OnFwd(OUT_A);OnRev(OUT_C);
#define turn_right0 OnFwd(OUT_A);Off(OUT_A);
#define turn_right(t) OnFwd(OUT_A);OnRev(OUT_C);Wait(t);Off(OUT_A);
#define turn_left0 OnFwd(OUT_C);Off(OUT_C);
#define turn_left1 OnFwd(OUT_C);OnRev(OUT_A);
#define turn_left(t) OnFwd(OUT_C);OnRev(OUT_A);Wait(t);Off(OUT_C);
#define go(t) OnRev(OUT_AC);Wait(t);Off(OUT_AC);
#define back(t) OnFwd(OUT_AC);Wait(t);Off(OUT_AC);
#define cross_stop Off(OUT_AC);Wait(100);
#define close SetPower(OUT_B,0);OnFwd(OUT_B);Wait(70);Off(OUT_B);
#define open SetPower(OUT_B,0);OnRev(OUT_B);Wait(70);Off(OUT_B);
#define STEP 1

sub Line()
{
    ClearTimer(0);
    while(FastTimer(0)<20){
        if(SENSOR_2>THRESHOLD +6) {
             turn_right1;
             ClearTimer(0);
         } else if (SENSOR_2>THRESHOLD +3) {
             turn_right0;
             ClearTimer(0);
         } else if (SENSOR_2>THRESHOLD ) {
             go_forward;
             ClearTimer(0);
         } else if (SENSOR_2>THRESHOLD -4) {
             turn_left0;
             ClearTimer(0); 
         } else {
             turn_left1;
          }
    Wait(STEP);
     }
    Off(OUT_AC);
}
sub LineFSPQ()
{
    ClearTimer(0);
    while(FastTimer(0)<17){
        if(SENSOR_2>THRESHOLD +6) {
             turn_right1;
             ClearTimer(0);
         } else if (SENSOR_2>THRESHOLD +3) {
             turn_right0;
             ClearTimer(0);
         } else if (SENSOR_2>THRESHOLD ) {
             go_forward;
             ClearTimer(0);
         } else if (SENSOR_2>THRESHOLD -4) {
             turn_left0;
             ClearTimer(0); 
         } else {
             turn_left1;
          }
    Wait(STEP);
     }
    Off(OUT_AC);
}
task main ()
{
   SetSensor(SENSOR_2,SENSOR_LIGHT);
   Line();
   cross_stop;
   go(20);
   turn_right(20);
   Line();
   turn_left(25);//CB
   go(13);
   Line();
   cross_stop;
   turn_left(25);
   go(30);//BP
   LineFSPQ();
   go(30);
   turn_right(50);//PQ
   Line();
   turn_left(10);//QR
   go(15);
   Line();
   cross_stop;
   turn_left(10);//RE
   Line();
   cross_stop;
   turn_left(18);
   go(8);
   Line();
   Line();
   Line();
   Line();
   turn_left(20);//EF
   go(10);
   LineFSPQ();
   cross_stop;
   go(30);
   turn_right(60);//FS
   go(50);
   close;
   back(40);
   turn_left(60);
   Line();
   cross_stop;
   go(30);//S_roll
   turn_right(20);
   LineFSPQ();
   cross_stop;
   turn_left(25);
   go(20);
   LineFSPQ();
   cross_stop;
   go(30);
   turn_right(105);
   go(23);
   open;
   back(25);
   turn_left(90);
   Line();
   Line();
   cross_stop;
   turn_left(50);
   go(10);
   Line();
   go(20);
}

トップ   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS