![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
[[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]]
