[[2004/C1/ロボコン]]

&size(25){最終版プログラム};

-[[タスク,関数の依存関係を表示...するつもりで書きました。>#task_list]]
-[[mainタスク>#main]]
-[[boot(初動の定義)>#boot]]
-[[pick(探索タスク)>#pick]]
-[[サブルーチン>#sub]]
-[[lift(持ち上げるタスク)>#lift]]

&aname(top);
 #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 )
 //=================================================================================================
 

&aname(task_list);
[[top>#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
      
&aname(main);
[[top>#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;
 }

&aname(boot);
[[top>#top]]  
 void Boot(void){
  Forward;
  Wait(200);
 }

&aname(pick);
[[top>#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
 
&aname(sub);
[[top>#top]] 
 sub mawaruRIGHT()
 {
  OnFwd(OUT_A);
  OnRev(OUT_C);
 }
 
 sub mawaruLEFT()
 {
  OnFwd(OUT_C);
  OnRev(OUT_A);
 }
 
&aname(lift);
[[top>#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>#top]]


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