#define zone1 37 //線の黒さ

#define zone2 45

#define zone3 60

#define zone4 65

#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 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 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_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);//通信の受け取り
       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);//通信を受け取る
       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); //打つ時のズレを修正

}


トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2013-08-11 (日) 18:00:57 (2205d)