2004/C1/ロボコン

最終版プログラム

#define RIGHT 1
#define LEFT -1
#define Forward OnFwd(OUT_A+OUT_C);
#define Back OnRev(OUT_A+OUT_C);

//=================LIFT======================================
#define HOME_HEIGHT FastTimer(3) > 500
#define HOME (SENSOR_1 < 60)
#define BOX ( ((730 < SENSOR_3) && (SENSOR_3 < 745)) || ((770 < SENSOR_3) && (SENSOR_3 < 800)) )
//#define Open OnFwd(OUT_B);Wait(200);
#define Close OnRev(OUT_B);Wait(200);

//730 745 white 770 800 green 820 840 normal
//============================================

//================LINE_TRACE======================================================================
#define bGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 )) && ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define wGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 )) || ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define lGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 ))
#define rGREEN ((720 < SENSOR_2) && (SENSOR_2 < 780 ))

#define bBLACK ( 780 < SENSOR_1) && ( 780 < SENSOR_2)
#define wBLACK ( 780 < SENSOR_1) || ( 780 < SENSOR_2)
#define lBLACK ( 780 < SENSOR_1)
#define rBLACK ( 780 < SENSOR_2)

#define bWHITE ( 720 > SENSOR_1 ) && ( 720 > SENSOR_2 )
#define wWHITE ( 720 > SENSOR_1 ) || ( 720 > SENSOR_2 )
#define lWHITE ( 720 > SENSOR_1 )
#define rWHITE ( 720 > SENSOR_2 )
//=================================================================================================

top //================task_function_sub_LIST=============================

/*
//task music();
task main();
 |_task Lift();
 |_task PickUp();
     |_task Grab();
         |_task Zenshin();

void Tsukamu(void);
void Boot(void);
sub Forward();
void PutBox(void);
*/
//=============================================================

int bring = 0;
int swch = 1;//course
     

top

task main(){
    SetSensor(SENSOR_1, SENSOR_LIGHT);//left-line_trace & home
    SetSensor(SENSOR_2, SENSOR_LIGHT);//right-line_trace
    SetSensor(SENSOR_3, SENSOR_LIGHT);//box-color_trace
    
    SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);//
    SetSensorMode(SENSOR_2, SENSOR_MODE_RAW);//
    SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);//0 ~ 1023
    
    //Boot();
    PlaySound(SOUND_DOUBLE_BEEP);
                start PickUp;
                start Lift;
}

top

void Boot(void){
 Forward;
 Wait(200);
}

top

//int swch = 0;
task PickUp()
{
while(true){
   if(swch == 1){
       while(true){
           Forward;
           until(wGREEN || BOX);
           if(rGREEN){
               Forward;
               until(lGREEN);
               mawaruLEFT();
               until(lWHITE);
               Forward;
               until(rWHITE);
               mawaruLEFT();
               until(bWHITE);
               swch++; break;
           }else if(lGREEN){
               Forward;
               until(lWHITE && rGREEN);
               mawaruLEFT();
               until(lGREEN);
               mawaruLEFT();
               until(bWHITE);
               swch++; break;
           }else if(BOX){    
               start Grab;
               stop PickUp;
           }else{
               PlaySound(SOUND_DOUBLE_BEEP);//Error
           }
       }//while
   }else if(swch == 2){
       while(true){
           Forward;
           until(wGREEN || wBLACK || BOX);
           if(rGREEN){
               mawaruRIGHT();
               until(bWHITE);
           }else if(lGREEN){
               mawaruLEFT();
               until(bWHITE);
           }else if(rBLACK){
               mawaruRIGHT();
               until(lBLACK && rWHITE);
               Forward;
               until(lWHITE && rBLACK);
               mawaruRIGHT();
               until(bWHITE);
               swch++; break;
           }else if(lBLACK){
               mawaruRIGHT();
               until(lBLACK && rWHITE);
               Forward;
               until(lWHITE && rBLACK);
               mawaruRIGHT();
               until(bWHITE);
               swch++; break;
           }else if(BOX){
          start Grab;
               stop PickUp;
           }else{
               PlaySound(SOUND_DOUBLE_BEEP);//Error
           }
       }//while
   }else if(swch == 3){
       while(true){
           Forward;
           until(wBLACK || rGREEN || BOX);
           if(rBLACK){
               mawaruRIGHT();
               until(bWHITE);
           }else if(lBLACK){
               mawaruLEFT();
               until(bWHITE);
           }else if(rGREEN){
               Forward;
               until(bWHITE);
           swch++; break;
           }else if(BOX){
               start Grab;
               stop PickUp;
           }else{
               PlaySound(SOUND_DOUBLE_BEEP);//Error
           }
       }//while
   }else if((4 <= swch) && (swch <= 6)){
       while(true){
           Forward;
           until(lBLACK || wGREEN);
           if(lBLACK){
               mawaruRIGHT();
               until(lBLACK && rWHITE);
               Forward;
               until(lWHITE);
               mawaruRIGHT();
               until(bWHITE);
               swch++; break;
           }else if(lGREEN){
               mawaruLEFT();
               until(lWHITE);
           }else if(rGREEN){
               mawaruRIGHT();
               until(bWHITE || lBLACK);
           }else if(BOX){    
               start Grab;
               stop PickUp;
           }else{
               PlaySound(SOUND_DOUBLE_BEEP);//Error
           }
       }//while
   }else if(swch == 7){
       while(true){
           Forward;
           until(lBLACK || wGREEN);
           if(lBLACK){
               Forward;
               until(lWHITE);
               mawaruLEFT();
               until(lBLACK);
               mawaruLEFT();
               until(bWHITE);
               swch++; break;
           }else if(lGREEN){
               mawaruLEFT();
               until(lWHITE);
           }else if(rGREEN){
               mawaruRIGHT();
               until(bWHITE || lBLACK);
           }else if(BOX){    
               start Grab;
               stop PickUp;
           }else{
               PlaySound(SOUND_DOUBLE_BEEP);//Error
           }
       }//while
   }else if(swch == 8){
       while(true){
           Forward;
           until(wBLACK || lGREEN);
           if(lBLACK){
               mawaruLEFT();
               until(lWHITE);
           }else if(rBLACK){
               mawaruRIGHT();
               until(bWHITE);
           }else if(lGREEN){
               Forward;
               until(lWHITE);
               mawaruLEFT();
               until(lGREEN);
               mawaruLEFT();
               until(bWHITE);
               swch++; break;
           }else if(BOX){    
               start Grab;
               stop PickUp;
           }else{
               PlaySound(SOUND_DOUBLE_BEEP);//Error
           }
       }//while
   }else if(swch == 9){
       ClearTimer(1);
           while(FastTimer(1) < 700){
               Forward;
               until(wGREEN || BOX);
               if(lGREEN){
                   mawaruLEFT();
                   until(lWHITE);
               }else if(rGREEN){
                   mawaruRIGHT();
                   until(bWHITE);
               }else if(BOX){    
                   start Grab;
                   stop PickUp;
               }else{
                   PlaySound(SOUND_DOUBLE_BEEP);//Error
               }//else
           }//while
       swch++;
   }else if(swch == 10){
       mawaruRIGHT();
       until(wGREEN);
       if(rGREEN){
           mawaruRIGHT();
           until(lGREEN);
           Forward;
           until(bWHITE);
           mawaruRIGHT();
           Wait(30);
       }else if(lGREEN){
           mawaruRIGHT();
           until(rGREEN);
       }
   Forward;
   until(HOME);
   swch = 1;
   }else{
       PlaySound(SOUND_DOUBLE_BEEP);
   }

}//while
}//pickup

top

sub mawaruRIGHT()
{
 OnFwd(OUT_A);
 OnRev(OUT_C);
}

sub mawaruLEFT()
{
 OnFwd(OUT_C);
 OnRev(OUT_A);
}

top

//=======================lift====================
int z = 1;//z == 0 --> Zenshin end

task Zenshin(){
   Forward;
   Wait(300);
   Off(OUT_A+OUT_C);
   z = 0;
   stop Zenshin;
}

task Open_Close(){
   while(true){
       if(z == 1){
           OnFwd(OUT_B);
       }else if(z == 0){
           OnRev(OUT_B);
       }
   }
}

task Grab(){
   OnFwd(OUT_B); Wait(100);
   start Zenshin;
   start Open_Close;
   Back; Wait(300);
   bring = 1;
   
   start PickUp;
   stop Zenshin;
   stop Open_Close;
   stop Grab;   
}

task Lift(){
    while(true){
                if(bring == true){
                         ClearTimer(3);
                         OnFwd(OUT_B);
                         until(HOME_HEIGHT);
                         Off(OUT_B);
                         break;
                }
    }
    while(true){
                if(HOME){
                         OnFwd(OUT_B); Wait(500);
                }
    }
}

top


トップ   編集 凍結 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2005-02-15 (火) 22:38:12