2007a/A9/ロボコン

全体のプログラム

上のRCXのプログラム

#define TURN_TIME 237           //180度曲がるのに要する大体の時間
#define TURN_TIME2 110          //90度曲がるのに要する大体の時間
#define COLLECT_TIME1 897       //1番上の箱の回収に要する時間
#define COLLECT_TIME 667        //その他の箱の回収に要する時間
#define BACK_TIME 30            //回転の時に箱をまきこまないように後退する時間
#define BACK_TIME2 130          //搭載部分を箱側にむけたあと、回収するための後退時間
#define TURN_MOVE 70            //90度まがるときに、回転の中心を交差点上にもってくる時間
task main()
{
 ClearMessage();
 while(true){
   if(Message()==1){                           //1のメッセージを受信したら
     OnRev(OUT_C);Wait(90);Off(OUT_C);         //後ろの腕?を開く
     Wait(BACK_TIME+BACK_TIME2+TURN_TIME-90);  //BACK_TIME+TURN_TIME+BACK_TIME2-90
     OnFwd(OUT_A+OUT_C);Wait(160);Off(OUT_C);
     Wait(100);
     Off(OUT_A);
     OnRev(OUT_C);Wait(90);Off(OUT_C);
     Wait(120);
     OnFwd(OUT_A+OUT_C);Wait(160);Off(OUT_C);
     Wait(110);Off(OUT_A+OUT_C);
     ClearMessage();
   }if(Message()==2){                         //2のメッセージを受信したら
     OnRev(OUT_C+OUT_B); Wait(170);           //BCを開いて箱を離す
     Off(OUT_C+OUT_B);                        //本体が箱を積んだ地点から離れるのを待つ
     Wait(250);
     OnFwd(OUT_C+OUT_B);Wait(70);Off(OUT_C);  //離れたので、腕?を閉じる 
     Wait(80);Off(OUT_B);
    ClearMessage();
   }if(Message()==3){
     OnRev(OUT_B+OUT_C);Wait(90);Off(OUT_C+OUT_B);   //後ろの腕?を広げる
     Wait(BACK_TIME+BACK_TIME2+TURN_TIME-90);        //BACK_TIME+BACK_TIME2+TURN_TIME-90
     OnFwd(OUT_A+OUT_C);Wait(70);
     OnFwd(OUT_B+OUT_A);Wait(40);Off(OUT_C);
     Wait(40);Off(OUT_B);
     Wait(250);
     Off(OUT_A);
     ClearMessage();
   }if(Message()==4){
     OnFwd(OUT_A+OUT_C);Wait(70);
     OnFwd(OUT_B+OUT_A);Wait(40);Off(OUT_C);
     Wait(40);Off(OUT_B);
     Wait(250);
     Off(OUT_A);
     ClearMessage();
   }
 }
}

下のRCXのプログラム

プログラム中の説明の座標の位置は↓↓で確認してください。

コース2.JPG
  • 移動コース
    (2,0)スタート→(1,0)の緑の箱を回収→(3,0)の緑の箱を回収→(4,0)の白の箱を回収
    (2,0)に戻って箱を置く→(3,0)経由で2つめの三段積みの箱集めに→
    (3,2)の緑の箱を回収→(1,2)の緑の箱を回収→(0,2)の白の箱を回収→
    (2,2)に戻って箱を置く=終了!!
  • プログラム /*ACは逆回転で前進し、Bは箱の上下に使うモーター*/
#define MOTOR OUT_A+OUT_C
#define THRESHOLD 40      //閾値
#define TURN_TIME 237     //180
#define TURN_TIME2 110    //90
#define COLLECT_TIME1 897 //1番上の箱の回収に要する時間
#define COLLECT_TIME 667  //その他の箱の回収に要する時間
#define BACK_TIME 30      //回転の時に箱をまきこまないように後退する時間
#define BACK_TIME2 130    //搭載部分を箱側にむけたあと、回収するための後退時間
#define BACK_TIME3 50     //箱を回収したあと、次の箱をまきこまないようにずらす時間
#define TURN_MOVE 70      //90度まがるときに、回転の中心を交差点上にもってくる時間 
int search_dir=0; 
int cross_count=0; 
int turn_count; 
int field_color;          //白1、黒0 
int trace_count;
int turn_count2;
sub turn_left()
{
OnRev(OUT_A);OnFwd(OUT_C);
}
sub turn_right()
{
OnFwd(OUT_A);OnRev(OUT_C);
}
  • 線を踏んだ数を数えて右折するtask(プログラムの説明は、ロボコンのページの「方向転換について」の部分をご覧下さい。)
    sub turn_right2()    //説明はロボコンのページの「方向転換について」をご覧ください・・・。
    {
    turn_count=0;
    field_color=0;
    while(turn_count<turn_count2){
    OnFwd(OUT_A);OnRev(OUT_C);
    if(field_color==0){           //最終色が黒
       if(SENSOR_1>THRESHOLD){
          field_color=1;
       }else{
          field_color=0;
       }
    }if(field_color==1){           //最終色が白
       if(SENSOR_1>THRESHOLD){
          field_color=1;
       }if(SENSOR_1<THRESHOLD){
          turn_count++; 
          field_color=0;
       }
     }
    }
    Off(OUT_A+OUT_C);
    }
  • 線を踏んだ数を数えて左折するtask
    sub turn_left2()
    {
    turn_count=0;
    field_color=0;
    while(turn_count<turn_count2){
    OnFwd(OUT_C);OnRev(OUT_A);
    if(field_color==0){           //最終色が黒
       if(SENSOR_1>THRESHOLD){
          field_color=1;
       }else{
          field_color=0;
       }
    }if(field_color==1){           //最終色が白
       if(SENSOR_1>THRESHOLD){
          field_color=1;
       }if(SENSOR_1<THRESHOLD){
          turn_count++; 
          field_color=0;
       }
    }
    }
    Off(OUT_A+OUT_C);
    }
task main() 
{
SetSensor(SENSOR_1,SENSOR_LIGHT);
SetSensor(SENSOR_2,SENSOR_LIGHT);
SetSensor(SENSOR_3,SENSOR_TOUCH);
ClearMessage();         
OnFwd(MOTOR);Wait(100);Off(MOTOR);
SendMessage(4);
OnFwd(OUT_B);Wait(400);Off(OUT_B);       //1個めの箱(1,0)回収終了
  trace_count=10; start line_trace;      //トレースで、交差点の数でとまらなくて良いときはtrace_countを10に設定(もちろん、もっと多くても良い)
until(SENSOR_3==1);                      //(3.0)の緑の箱を認識
  stop line_trace;
  ClearTimer(0);
  start box_collect;           //箱の回収開始
    until(FastTimer(0)>COLLECT_TIME);    //箱の回収が終了したら 
  stop box_collect;
ClearTimer(0);
start line_trace2;
until(FastTimer(0)>200);  stop line_trace2; Wait(270);
cross_count=0;                           //交差点の数を数えて円に向かうので、カウントリセット
ClearTimer(0);
  trace_count=5;
  start line_trace;                      //円に向かう
until(FastTimer(0)>600);
  stop line_trace;
  OnRev(MOTOR); Wait(TURN_MOVE+35);      //90度曲がるので、回転の中心を交差点に近づける
  turn_count2=1; turn_left2();           //90度左折して円の側に後ろを向ける。
  OnFwd(MOTOR); Wait(50); Off(MOTOR);    //円の中に確実に置けるように後退させる
ClearMessage();
SendMessage(2);                          //箱を離してください(通信)
 Wait(300);                              //箱を離して一端停止ClearTimer(0);
 trace_count=1;
 start line_trace;                       //回転の中心を線上(2,0)にもってこれるように前進。
until(FastTimer(0)>200);
 stop line_trace; turn_count2=1; turn_left();   //90度左折
cross_count=0;                           //1つめの交差点を目指す
  trace_count=2;
  start line_trace;
  Wait(130);
  stop line_trace;                       //1つ目の交差点(3,0)にさしかかる
  OnRev(MOTOR); Wait(TURN_MOVE);         //90度方向転換するので回転の中心を交差点に近づける 
  turn_count2=1; turn_left();            //90度左折して(3,2)に向かう
  trace_count=10;
  start line_trace;
until(SENSOR_3==1);                      //1つ目の(3,2)緑の箱を感知
 ClearTimer(0);
  stop line_trace;
  start box_collect;
    until(FastTimer(0)>COLLECT_TIME);    //箱を認識してから回収が終了 
  stop box_collect;ClearTimer(0);
start line_trace2;
until(FastTimer(0)>200); 
stop line_trace2; Wait(270);
cross_count=0;                           //交差点の数を数えて円に向かうので、カウントリセット 
ClearTimer(0);
  trace_count=5;
  start line_trace;                      //円に向かう
until(FastTimer(0)>600);
  stop line_trace;
  OnRev(MOTOR); Wait(TURN_MOVE+35);      //90度曲がるので、回転の中心を交差点に近づける
  turn_count2=1; turn_left();            //90度左折して円の側に後ろを向ける。
  OnFwd(MOTOR); Wait(50); Off(MOTOR);    //円の中に確実に置けるように後退させる
ClearMessage();
SendMessage(2);                          //箱を離してください(通信)
 Wait(300);                              //箱を離して一端停止
ClearTimer(0);
 trace_count=2;
 start line_trace;                       //回転の中心を線上(2,0)にもってこれるように前進。
until(FastTimer(0)>130);
 stop line_trace;Off(OUT_A+OUT_B+OUT_C);
}
task box_collect()                       //ライントレースを停止して箱を回収する
{
 ClearMessage();
   stop line_trace;
 SendMessage(1);                         //上のRCXに通信。箱の回収
   OnFwd(MOTOR); Wait(BACK_TIME); Off(MOTOR);             //トレースを停止して後退
   turn_count2=2; turn_right();                           //180度右回転
   OnFwd(MOTOR); Wait(BACK_TIME2); Off(MOTOR); Wait(270); //上のBが開いた状態で後退してもう一方のRCXのほうの回収時間の経過を待つ。
}
task box_collect1()                              //ライントレースを停止して箱を回収する(一番上専用)
{
ClearMessage();
   stop line_trace;
SendMessage(3);                                 //上のRCXに通信。箱の回収
   OnFwd(MOTOR); Wait(BACK_TIME); Off(MOTOR);   //トレースを停止して後退
   turn_count2=2; turn_right();                 //180度右回転
   OnFwd(MOTOR); Wait(BACK_TIME2); Off(MOTOR);  //上のBが開いた状態で後退
   OnRev(OUT_B);Wait(400); Off(OUT_B);          //モーターBを動かす時間(4)秒
}
  • ライントレースをしながら後退するtask(プログラムの説明についてはライントレースのページをご覧下さい。)
    task line_trace2()                              //バックライントレース
    {
    SetSensor(SENSOR_1,SENSOR_LIGHT);
    SetSensor(SENSOR_2,SENSOR_LIGHT);OnFwd(MOTOR);  //最初の動きとして直進させる。
    while(true){
     if(SENSOR_1<THRESHOLD)                         //線を見失ったら
     {
      if(search_dir==0){
       turn_right(); Wait(5);
        if(SENSOR_1>THRESHOLD){                     //線をみつけたのなら直進
            OnFwd(MOTOR); 
            search_dir=0;
        }else{                                      //見付からないのなら反対側を探す
            turn_left(); Wait(10);
             if(SENSOR_1>THRESHOLD){                //見付けたのなら直進
               OnFwd(MOTOR);
               search_dir=1;
             }else{                                 //見付からないのなら1秒前進
               OnFwd(MOTOR);Wait(100); 
               search_dir=0;
             }
        }
      }
      if(search_dir==1){
        turn_left(); Wait(5);
          if(SENSOR_1>THRESHOLD){
             OnFwd(MOTOR);
             search_dir=1;
          }else{
             turn_right(); Wait(10);
              if(SENSOR_1>THRESHOLD){
                OnFwd(MOTOR);
                search_dir=0;
              }else{
                OnFwd(MOTOR); Wait(100);
                search_dir=1;
              }
          }
       }
     }
    }
    Off(OUT_A+OUT_C);
    }
  • ライントレースをするtask
    task line_trace()                       //ライントレース
    {
    cross_count=0; OnRev(MOTOR);            //最初の動きとして直進させる。
    while(cross_count<trace_count){
     if(SENSOR_2<THRESHOLD) {
      cross_count++;
     if(SENSOR_1<THRESHOLD)                 //線を見失ったら
     {
      if(search_dir==0){
       turn_right(); Wait(5);
        if(SENSOR_1>THRESHOLD){             //線をみつけたのなら直進
            OnRev(MOTOR); 
            search_dir=0;
        }else{                              //見付からないのなら反対側を探す
            turn_left(); Wait(10);
             if(SENSOR_1>THRESHOLD){        //見付けたのなら直進
               OnRev(MOTOR);
               search_dir=1;
             }else{                         //見付からないのなら1秒前進
               OnRev(MOTOR);Wait(100);
               search_dir=0;
             }
        }
      }
      if(search_dir==1){
        turn_left(); Wait(5);
          if(SENSOR_1>THRESHOLD){
             OnRev(MOTOR);
             search_dir=1;
          }else{
             turn_right(); Wait(10);
              if(SENSOR_1>THRESHOLD){
                OnRev(MOTOR);
                search_dir=0;
              }else{
                OnRev(MOTOR); Wait(100);
                search_dir=1;
              }
          }
       }
      }
     }else{
     if(SENSOR_1<THRESHOLD)              //線を見失ったら
     {
      if(search_dir==0){
       turn_right(); Wait(5);
        if(SENSOR_1>THRESHOLD){          //線をみつけたのなら直進
            OnRev(MOTOR); 
            search_dir=0;
        }else{                           //見付からないのなら反対側を探す
            turn_left(); Wait(10);
             if(SENSOR_1>THRESHOLD){     //見付けたのなら直進
               OnRev(MOTOR);
               search_dir=1;
             }else{                      //見付からないのなら1秒前進
               OnRev(MOTOR);Wait(100);
               search_dir=0;
             }
        }
      }
      if(search_dir==1){
        turn_left(); Wait(5);
          if(SENSOR_1>THRESHOLD){
             OnRev(MOTOR);
             search_dir=1;
          }else{
             turn_right(); Wait(10);
              if(SENSOR_1>THRESHOLD){
                OnRev(MOTOR);
                search_dir=0;
              }else{
                OnRev(MOTOR); Wait(100);
                search_dir=1;
              }
          }
       }
      }
     }
    }
    Off(OUT_A+OUT_C);
    }

コメントをどうぞ〜



添付ファイル: fileコース2.JPG 211件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2007-08-09 (木) 17:22:29 (4398d)