#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);
 }

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