#define zone1 37
#define zone1 37 //線の黒さ
#define zone2 45
#define zone3 60
#define zone4 65
#define SPEED 40
#define SPEED 40 //モーターのスピード
#define lowSPEED SPEED*0.625
#define zensin OnFwd(OUT_BC,SPEED*0.8);
#define sasetu OnFwd(OUT_C,SPEED);Off(OUT_B);
#define kyuusasetu OnFwd(OUT_C,SPEED);OnRev(OUT_B,lowSPEED);
#define zensin OnFwd(OUT_BC,SPEED*0.8); //まっすぐ進む
#define sasetu OnFwd(OUT_C,SPEED);Off(OUT_B); //左に曲がる
#define kyuusasetu OnFwd(OUT_C,SPEED);OnRev(OUT_B,lowSPEED); //より左に曲がる
#define STEP 1
#define nMAX 100
#define SIGNAL1 11
#define SIGNAL2 12 
#define usetu OnFwd(OUT_B,SPEED);Off(OUT_C);
#define kyuuusetu OnFwd(OUT_B,SPEED);OnRev(OUT_C,lowSPEED);
#define CONN 1
#define back RotateMotor(OUT_BC,-SPEED,80);Off(OUT_BC);
#define SIGNAL1 11 //通信のメッセージ
#define SIGNAL2 12 //通信のメッセージ
#define usetu OnFwd(OUT_B,SPEED);Off(OUT_C); //右に曲がる
#define kyuuusetu OnFwd(OUT_B,SPEED);OnRev(OUT_C,lowSPEED); //より右に曲がる
#define back RotateMotor(OUT_BC,-SPEED,80);Off(OUT_BC); //後退する
#define M 1
#define M1 5

int nOnline=0;
  int mOnline=0;
  int n=0;
  int m=0;
  int r=0;
  int q=0;
  int msg=23;
  

sub migigawa(int x) //交差点をx回通過するまで右側をライントレースし続けるよ
        {
         while(m < x){


            while(nOnline < nMAX && m<x) {
                if (SENSOR_4 < zone1) {
                OnFwd(OUT_B,SPEED);OnRev(OUT_C,lowSPEED); //小回りな右折
                nOnline++;
          } else {
              if (SENSOR_4 < zone2) {
                OnFwd(OUT_B,SPEED);Off(OUT_C); //普通の右折
            } else if (SENSOR_4 < zone3) {
                zensin;
            } else if (SENSOR_4 < zone4) {
                OnFwd(OUT_C,SPEED);Off(OUT_B); // 普通の左折
            } else {
                OnFwd(OUT_C,SPEED);OnRev(OUT_B,lowSPEED); //小回りな左折
               }
                nOnline=0;
               }
                Wait(STEP);
               }
                OnFwd(OUT_C,SPEED);OnRev(OUT_B,lowSPEED); //小回りな左折
                Wait(nMAX*STEP);
                OnFwd(OUT_BC,SPEED);Wait(300);
                nOnline=0;
                m++;
               }
                m=0;
 }



    sub hidarigawa(int x) //交差点をx回通過するまでライントレースし続けるよ
      {
        while(m < x) {

            while(nOnline < nMAX && m<x) {
                if (SENSOR_4 < zone1) {
                OnFwd(OUT_C,SPEED);Off(OUT_B); // 普通の左折
                nOnline++;
          } else {
              if (SENSOR_4 < zone2) {
                OnFwd(OUT_C,SPEED);OnRev(OUT_B,lowSPEED); //小回りな左折
            } else if (SENSOR_4 < zone3) {
                zensin;
            } else if (SENSOR_4 < zone4) {
                OnFwd(OUT_B,SPEED);Off(OUT_C); //普通の右折
            } else {
                OnFwd(OUT_B,SPEED);OnRev(OUT_C,lowSPEED); //小回りな右折
               }
                nOnline=0;
               }
                Wait(STEP);
               }
                OnFwd(OUT_B,SPEED);OnRev(OUT_C,lowSPEED);
                Wait(nMAX*STEP);
                OnFwd(OUT_BC,SPEED);Wait(300);
                nOnline=0;
                m++;
               }
                m=0;
 }

















task main ()
{    
  SetSensorLight(S4);  
  


  Wait(3000);
  while (m<1) {//一周目、最初のライントレース

    while(nOnline < nMAX && m<1) {
      if (SENSOR_4 < zone1){
      kyuusasetu;
      nOnline++;
    } else {
    if (SENSOR_4 < zone2){
         sasetu;
     } else if (SENSOR_4 < zone3){
         zensin;
      } else if (SENSOR_4 < zone4){
         usetu;
      } else {
         kyuuusetu;
       }
      nOnline=0;
     }
       Wait(STEP);
    }
     kyuusasetu;Wait(nMAX*STEP);
      OnFwd(OUT_BC,SPEED);Wait(300);
      nOnline=0;
       m++;
}
    m=0;
     RotateMotor(OUT_BC,SPEED,50);//方向転換
     Off(OUT_BC);
     Wait(1000);
     RotateMotor(OUT_B,-SPEED,230);

     Off(OUT_BC);
     Wait(2000);
     
    while (r == 0){
        ReceiveRemoteNumber(MAILBOX1, false ,msg);//受け取り1回目
        if (msg == SIGNAL1){
            PlaySound(SOUND_UP);
            OnRev(OUT_A,20);
            Wait(1500);
            Off(OUT_A);
            r=65;
        } 
        Wait(100);
     }

    RotateMotor(OUT_C,-SPEED,180);//中間地点に移動
    RotateMotor(OUT_B,-SPEED,400);
    RotateMotor(OUT_BC,SPEED,300);
     RotateMotor(OUT_B,SPEED,20);
    //OnRev(OUT_BC,SPEED);until(SENSOR_4 < 50);Off(OUT_BC);
     
    r=77;

    while (r == 77){
        ReceiveRemoteNumber(MAILBOX2,false,msg);//打ち出し1回目
        if(msg == SIGNAL2){
             PlaySound(SOUND_UP);
                OnFwd(OUT_BC,SPEED);
                RotateMotor(OUT_A,60,330);
                Off(OUT_ABC);
                RotateMotor(OUT_BC,-SPEED,200);
             r=22;
           }
            Wait(100);
          }

    Wait(1500);
    OnRev(OUT_BC,SPEED);until(SENSOR_4 < 50);Off(OUT_BC);//打ってから黒線まで戻る
    RotateMotor(OUT_BC,SPEED,70);OnFwd(OUT_C,SPEED);OnRev(OUT_B,SPEED);
    until(SENSOR_4 < 50);Off(OUT_BC);
    
     

  while (q<1) {//一周目、2回目のライントレース

    while(nOnline < nMAX && q<1) {
      if (SENSOR_4 < zone1){
      kyuuusetu;
      nOnline++;
    } else {
    if (SENSOR_4 < zone2){
         usetu;
     } else if (SENSOR_4 < zone3){
         zensin;
      } else if (SENSOR_4 < zone4){
         sasetu;
      } else {
         kyuusasetu;
       }
      nOnline=0;
     }
       Wait(STEP);
    }
     kyuuusetu;Wait(nMAX*STEP);
      OnFwd(OUT_BC,SPEED);Wait(300);
      nOnline=0;
      q++;
}

    RotateMotor(OUT_BC,SPEED,350);RotateMotor(OUT_B,SPEED,400);
    RotateMotor(OUT_BC,SPEED,350);RotateMotor(OUT_B,SPEED,400);//二回目受け取りの位置まで移動
    RotateMotor(OUT_C,-SPEED,180);Off(OUT_BC);RotateMotor(OUT_C,-SPEED,80);
   
    r=24;

    while(r == 24){
        ReceiveRemoteNumber(MAILBOX3, false ,msg);//受け取り2回目
        if (msg == SIGNAL1){
            PlaySound(SOUND_UP);
            OnRev(OUT_A,20);
            Wait(1500);
            Off(OUT_A);
            r=23;
         }
          Wait(100);
      }
        

        


 Wait(1000);
 OnRev(OUT_BC,SPEED);until(SENSOR_4 < 50);Off(OUT_BC);//黒線を探す
 //RotateMotor(OUT_BC,SPEED,400);
 //RotateMotor(OUT_B,-SPEED,650);//曲線部分から直線部分へ
 //RotateMotor(OUT_BC,-SPEED,280);
 RotateMotor(OUT_BC,SPEED,40);OnFwd(OUT_C,SPEED);OnRev(OUT_B,SPEED);
 until(SENSOR_4 < 50);Off(OUT_BC);


  m=0;//一周目、最後のライントレース
  while (m<1) {

    while(nOnline < nMAX && m<2) {
      if (SENSOR_4 < zone1){
     kyuuusetu;
      nOnline++;
    } else {
    if (SENSOR_4 < zone2){
         usetu;
     } else if (SENSOR_4 < zone3){
         zensin;
      } else if (SENSOR_4 < zone4){
         sasetu;
      } else {
         kyuusasetu;
      }
      nOnline=0;
     }
       Wait(STEP);
    }
     kyuusasetu;Wait(nMAX*STEP);
     OnFwd(OUT_BC,SPEED);Wait(300);
     nOnline=0;
     m++;
}
     Off(OUT_BC);
     RotateMotor(OUT_B,-SPEED,380);
     RotateMotor(OUT_A,100,300);//打ち出し2回目
     m=0;









     RotateMotor(OUT_BC,-SPEED,100);//二週目開始
      OnRev(OUT_BC,SPEED);until(SENSOR_4 < 50);
      Off(OUT_BC);
      RotateMotor(OUT_BC,SPEED,150);
      RotateMotor(OUT_B,-SPEED,650);//曲線部分から直線部分まで
      RotateMotor(OUT_BC,-SPEED,400);
      q=0;
     while (q<1) {

    while(nOnline < nMAX && q<1) {
      if (SENSOR_4 < zone1){
      kyuuusetu;
      nOnline++;
    } else {
    if (SENSOR_4 < zone2){
         usetu;
     } else if (SENSOR_4 < zone3){
         zensin;
      } else if (SENSOR_4 < zone4){
         sasetu;
      } else {
         kyuusasetu;
       }
      nOnline=0;
     }
       Wait(STEP);
    }
     kyuuusetu;Wait(nMAX*STEP);
      OnFwd(OUT_BC,SPEED);Wait(300);
      nOnline=0;
      q++;
}


    








Off(OUT_BC);Wait(1000);
    RotateMotor(OUT_C,-SPEED,200);RotateMotor(OUT_B,SPEED,100);Off(OUT_BC); //角度調整
    zensin;OnRev(OUT_A,20);Wait(1500);Off(OUT_A); //アームで球を捕獲
    zensin;until(SENSOR_4 < 50);Off(OUT_BC); //黒線まで直進
    OnRev(OUT_BC,SPEED);Wait(500);OnRev(OUT_BC,SPEED);until(SENSOR_4 < 50); //後ろの黒線までバック
    OnFwd(OUT_B,SPEED);OnRev(OUT_C,SPEED);Wait(500); //旋回
    OnFwd(OUT_B,SPEED);OnRev(OUT_C,SPEED);until(SENSOR_4 < 50); //ライントレース出来る体制に戻る

    hidarigawa(1); //左側をライントレースしながら交差点を一回カウント

    Off(OUT_BC);Wait(1000);RotateMotor(OUT_BC,-SPEED,430);Wait(1000); //打つ地点までバック
    RotateMotor(OUT_B,-SPEED,300);RotateMotor(OUT_C,SPEED,70);Off(OUT_BC); //角度調整
    


        
    r=24;
while(r == 24){
        ReceiveRemoteNumber(MAILBOX4, false ,msg);//
        ReceiveRemoteNumber(MAILBOX4, false ,msg);//通信の受け取り
        if (msg == SIGNAL2){
            PlaySound(SOUND_UP);
            r=23;
         }
          Wait(100);
      }








   Wait(1000);
   //OnFwd(OUT_BC,SPEED+10);
   RotateMotor(OUT_A,100,400);Off(OUT_ABC);//打つ  
   //RotateMotor(OUT_BC,-SPEED,200);
    OnRev(OUT_BC,SPEED);until(SENSOR_4 < 50);RotateMotor(OUT_BC,SPEED,50); //打った状態から元に戻るその一
    OnFwd(OUT_B,SPEED);OnRev(OUT_C,SPEED);until(SENSOR_4 < 50); //打った状態から元に戻るその二
    Off(OUT_BC); //五秒待つ

    hidarigawa(1); //左側ライントレース交差点を一回カウント

    RotateMotor(OUT_BC,SPEED,100);Off(OUT_BC); //受け取る場所まで移動
    RotateMotor(OUT_BC,-SPEED,10);RotateMotor(OUT_C,SPEED,550);Off(OUT_BC); //90°旋回



        r=24;
while(r == 24){
        ReceiveRemoteNumber(MAILBOX5, false ,msg);//受け取り2回目
        if (msg == SIGNAL1){
            PlaySound(SOUND_UP);
            r=23;
         }
          Wait(100);
      }




    OnRev(OUT_A,20);Wait(1500);Off(OUT_A); //球を受け取る

    OnRev(OUT_BC,SPEED);until(SENSOR_4 < 50);Off(OUT_BC);RotateMotor(OUT_BC,SPEED,70);
    OnFwd(OUT_B,SPEED);OnRev(OUT_C,SPEED);until(SENSOR_4 < 50); //ライントレース出来る体制に戻る
    Off(OUT_BC); //五秒待つ

    hidarigawa(1); //左側、交差点を一回カウント

    RotateMotor(OUT_BC,SPEED,50);Off(OUT_BC); //打つ地点まで移動
    RotateMotor(OUT_B,-SPEED,410);Off(OUT_B); //角度調整
    
  
        r=24;
while(r == 24){
        ReceiveRemoteNumber(MAILBOX6, false ,msg);
        ReceiveRemoteNumber(MAILBOX6, false ,msg);//通信を受け取る
        if (msg == SIGNAL2){
            PlaySound(SOUND_UP);
            r=23;
         }
          Wait(100);
      }








    //OnFwd(OUT_BC,SPEED+10);
    RotateMotor(OUT_A,100,270);Off(OUT_A); //打つ
    RotateMotor(OUT_BC,-SPEED,50);Off(OUT_BC); //打った時のズレを修正
    RotateMotor(OUT_BC,-SPEED,50);Off(OUT_BC); //打つ時のズレを修正




}

トップ   編集 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS