#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