2009b/A1/ロボコン

#define go OnFwd(OUT_A+OUT_C);   //前進
#define back OnRev(OUT_A+OUT_C); //後退
#define tomaru Off(OUT_A+OUT_C); //停止
#define turn_l OnFwd(OUT_A);     //左折
#define turn_r OnFwd(OUT_C);     //右折
#define shine 45                 //ライントレースのための光の値
#define up(t) OnFwd(OUT_B);Wait(t);Off(OUT_B);  //アームを上げる
#define down(t) OnRev(OUT_B);Wait(t);Off(OUT_B);//アームを下げる


task follow_line_1(){  //交差点を直進するプログラム
while(true){
          if((SENSOR_3 >= shine)&&(SENSOR_1 >= shine)){
             go;
             until((SENSOR_3 < shine)||(SENSOR_1 < shine));
             tomaru;
           }
          else if((SENSOR_3 < shine)&&(SENSOR_1 >= shine)){
             turn_l;
             until((SENSOR_3 >= shine)&&(SENSOR_1 >= shine));
             tomaru;
           }
          else if((SENSOR_3 >= shine)&&(SENSOR_1 < shine)){
             turn_r;
             until(((SENSOR_3 >= shine)&&(SENSOR_1 >= shine))||((SENSOR_3 < shine)&&(SENSOR_1 < shine)));
             tomaru;
           }
          else if((SENSOR_3 < shine)&&(SENSOR_1 < shine)){
             PlaySound(SOUND_UP);
             turn_l;
             until((SENSOR_3 >= shine)&&(SENSOR_1 >= shine));
             tomaru;
           }
         }
}
task follow_line_2(){  //交差点を右折するプログラム_場合1
while(true){
          if((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)){
             go;
             until((SENSOR_1 < shine)||(SENSOR_3 < shine));
             tomaru;
           }
          else if((SENSOR_1 < shine)&&(SENSOR_3 >= shine)){
             turn_r;
             until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
             tomaru;
           }
          else if((SENSOR_1 >= shine)&&(SENSOR_3 < shine)){
             turn_l;
             until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
             tomaru;
           }
          else if((SENSOR_1 < shine)&&(SENSOR_3 < shine)){
             PlaySound(SOUND_UP);
             OnFwd(OUT_C);OnRev(OUT_A);
             until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
             tomaru;
           }
         }
} 
task follow_line_3(){  //交差点を右折するプログラム_場合2
PlaySound(1);
while(true){
          if((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)){
             go;
             until((SENSOR_1 < shine)||(SENSOR_3 < shine));
             tomaru;
           }
          else if((SENSOR_1 < shine)&&(SENSOR_3 >= shine)){
             turn_r;
             until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
             tomaru;
           }
          else if((SENSOR_1 >= shine)&&(SENSOR_3 < shine)){
             turn_l;
             until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
             tomaru;
           }
          else if((SENSOR_1 < shine)&&(SENSOR_3 < shine)){
             PlaySound(SOUND_UP);
             OnFwd(OUT_C);OnRev(OUT_A);
             ClearTimer(0);                 //交差点を曲がるとき曲がりすぎないように曲がる時間の最大を決める
             until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||(FastTimer(0) > 70));          
             tomaru;
           }
         }
}

task follow_line_4(){  //交差点を左折するプログラム
int k=0;
while(true){
          if((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)){
             go;
             until((SENSOR_1 < shine)||(SENSOR_3 < shine));
             tomaru;
           }
          else if((SENSOR_1 < shine)&&(SENSOR_3 >= shine)){
             turn_r;
             until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
             tomaru;
           }
          else if((SENSOR_1 >= shine)&&(SENSOR_3 < shine)){
             turn_l;
             until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
             tomaru;
           }
          else if(((SENSOR_1 < shine)&&(SENSOR_3 < shine))&&(k<1)){
             PlaySound(SOUND_UP);k++;      //交差点の回数を数える
             OnFwd(OUT_A);OnRev(OUT_C);
             until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
             tomaru;
           }
          else if(((SENSOR_1 < shine)&&(SENSOR_3 < shine))&&(k==1)){
             tomaru;                      //下図の交差点1を一回曲がってスタート地点まで来たら止まる
           }
         }
}

task follow_line_5(){  //狭い通路を通るためのプログラム
while(true){
          if(SENSOR_1 >= shine){
             turn_l;
             until(SENSOR_1 < shine);
             tomaru;
           }
          else if(SENSOR_1 < shine){
             turn_r;
             until(SENSOR_1 >= shine);
             tomaru;
           }
         }
}

task main(){
   SetSensor(SENSOR_1,SENSOR_LIGHT);//センサー1を光センサーに決める
   SetSensor(SENSOR_2,SENSOR_TOUCH);//センサー2をタッチセンサーに決める
   SetSensor(SENSOR_3,SENSOR_LIGHT);//センサー3を光センサーに決める

   start follow_line_1;		//スタート地点から紙パックの横まで進む
   Wait(460);
   stop follow_line_1;
   tomaru;
   OnFwd(OUT_C);OnRev(OUT_A);	//紙パックをとるために右に90度方向変換
   Wait(100);
   tomaru;
   go;				//紙パックに当たるまで進む
   until(SENSOR_2 == 1);
   tomaru;
   down(70);			//紙パックをつかむ
   go;
   Wait(20);
   tomaru;
   down(70);
   back;				//ライン上に戻るまでバック
   until((SENSOR_1 < shine)&&(SENSOR_3 < shine));
   tomaru;
   OnFwd(OUT_A);OnRev(OUT_C);
   Wait(60);
   tomaru;
   start follow_line_1;		//ライントレースを開始
   Wait(500);
   stop follow_line_1;
   tomaru;
   up(200);
   go;				//紙パックを邪魔にならないところへ置いてくる
   Wait(200);                      //(下図の紙パックと書かれているところへ置いていく)
   tomaru;
   back;
   Wait(235);
   tomaru;
   OnFwd(OUT_C);OnRev(OUT_A);
   Wait(110);
   tomaru;
   go;
   Wait(50);
   tomaru;
   start follow_line_5;		//一周回ってスタート地点へ戻る
   Wait(300);
   stop follow_line_5;
   tomaru;
   start follow_line_2;             //下図の交差点3を通過する
   Wait(900);
   stop follow_line_2;
   tomaru;
   start follow_line_3;             //下図の交差点4を通過する
   Wait(800);
   stop follow_line_3;
   tomaru;
   start follow_line_4;             //下図の交差点1を通過する
   Wait(600);
   stop follow_line_4;
   tomaru;
   OnFwd(OUT_A);			//もう一台入れるように綺麗にスタート地点に入る
   Wait(15);
   tomaru;
   go;until(SENSOR_2 == 1);tomaru;
   back;Wait(55);tomaru;
   OnFwd(OUT_A);OnRev(OUT_C);
   Wait(90);
   tomaru;
   go;
   until((SENSOR_1 < shine)&&(SENSOR_3 < shine));
   tomaru;
}

ロボットの進む経路

サイズ,進む経路


添付ファイル: file無題41.jpg 118件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2010-02-13 (土) 19:34:50 (3480d)