| 0,0 → 1,203 |
|---|
| // Soutezni robot pro Istrobot 2004 |
| // Objeti cihly vpravo |
| // $Archive: /Lego/Callis/Callis_R.nqc $ |
| // $Date: 27.04.04 20:52 $ |
| // $Revision: 3 $ |
| |
| #define THRESHOLD 50 // rozhodovaci uroven mezi cernou a bilou |
| #define ML OUT_A // Motory |
| #define MR OUT_C |
| #define FL OnFwd(ML) // Vpred |
| #define FR OnRev(MR) |
| #define BL OnRev(ML) // Vzad |
| #define BR OnFwd(MR) |
| #define RSENSOR SENSOR_3 // Senzory na caru |
| #define LSENSOR SENSOR_1 |
| #define BUMPER SENSOR_2 // Senzor na cihlu |
| #define MEZERA 38 // jak se muze jet dlouho bez cary |
| #define COUVEJ 70 // kolik se ma couvat po detekci diry |
| #define CIKCAK 40 // kolik se ma jet cik/cak |
| #define PRES_DIRU 35 // jak predpokladame velkou diru |
| |
| #define L 0 // left |
| #define R 1 // right |
| #define S 2 // straight |
| |
| #define STOP Off(ML + MR); Wait(10); // zastav 8 |
| |
| int movement; // smer minuleho pohybu |
| int line; // na ktere strane byla detekovana cara |
| int dira; // pocitadlo pro nalezeni preruseni cary |
| int n; // pomocna promena pro cyklus FOR |
| |
| |
| void cikcak() // Hledani cary |
| { |
| n=CIKCAK/2; // poprve hned zatoc opacne, nez se jelo pred detekci diry |
| switch(movement) // zmenime smer zataceni |
| { |
| case L: |
| FL; BR; // doprava |
| movement=R; // poznamenej kam jedem |
| line=L; // kdyz prejedem, tak bude cara vlevo |
| break; |
| case R: |
| FR; BL; // doleva |
| movement=L; // poznamenej kam jedem |
| line=R; // kdyz prejedem, tak bude cara vpravo |
| break; |
| } |
| |
| while(true) // jed cik-cak, dokud nenajdes caru |
| { |
| if ((THRESHOLD < RSENSOR) || (THRESHOLD < RSENSOR)) // je cara ? |
| { |
| STOP; |
| break; |
| }; |
| if (CIKCAK < n++) // Jedeme uz dost dlouho cik? Pak jed cak. |
| { |
| n=0; |
| STOP; |
| switch(movement) // zmenime smer zataceni |
| { |
| case L: |
| FL; BR; // doprava |
| movement=R; // poznamenej kam jedem |
| line=L; // kdyz prejedem, tak bude cara vlevo |
| break; |
| case R: |
| FR; BL; // doleva |
| movement=L; // poznamenej kam jedem |
| line=R; // kdyz prejedem, tak bude cara vpravo |
| break; |
| } |
| } |
| } |
| } |
| |
| task main() |
| { |
| PlaySound (SOUND_DOUBLE_BEEP); |
| Wait(100); // 1s |
| SetSensor(RSENSOR,SENSOR_LIGHT); // senzor na caru modry pravy |
| SetSensor(LSENSOR,SENSOR_LIGHT); // senzor na caru modry levy |
| SetSensor(BUMPER,SENSOR_TOUCH); // sensor na prekazku |
| |
| SetPower (ML,OUT_FULL); // vykon motoru |
| SetPower (MR,OUT_FULL); |
| movement=R; |
| line=S; |
| dira=0; |
| |
| cikcak(); // toc se, abys nasel caru |
| FL; FR; // vpred |
| |
| // Sledovani cary |
| while(true) |
| { |
| if(BUMPER) // je cihla? |
| { |
| PlaySound(SOUND_DOWN); |
| objizdka(); // objed cihlu |
| } |
| |
| if (THRESHOLD < RSENSOR) // Cara pod pravym senzorem |
| { |
| dira=0; // nuluj pocitadlo diry, protoze jsme videli caru |
| line=R; // zaznamenej, kdes videl caru |
| FL; FR; // rovne |
| continue; |
| } |
| |
| if (THRESHOLD < LSENSOR) // Cara pod levym senzorem |
| { |
| dira=0; // nuluj pocitadlo diry, protoze jsme videli caru |
| line=L; // zaznamenej, kdes videl caru |
| FL; FR; // rovne |
| continue; |
| } |
| |
| // oba senzory mimo caru |
| |
| if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat |
| // v ostatnich cyklech nedelame nic (pro urychleni snimani) |
| { |
| switch(line) // kdyz nevidis caru, tak jed tam, kdes ji videl naposled |
| { |
| case L: |
| Off(ML); // doleva |
| movement=L; // poznamenej kam jedes |
| break; |
| case R: |
| Off(MR); // doprava |
| movement=R; // poznamenej kam jedes |
| break; |
| }; |
| dira++; // zvets pocitadlo diry, aby to pristi cyklus tudy neslo |
| continue; // co nejrychleji se vrat na snimani cary |
| } |
| |
| if (MEZERA < dira++) // nejedeme uz moc dlouho bez cary? |
| { |
| dira=0; |
| PlaySound (SOUND_FAST_UP); |
| switch(movement) // kam se jelo ? |
| { |
| case L: |
| Off(ML); // couvej doprava |
| BR; |
| Wait(COUVEJ+10); |
| movement=R; |
| break; |
| case R: |
| BL; // couvej doleva |
| Off(MR); |
| Wait(COUVEJ); |
| movement=L; |
| break; |
| } |
| PlaySound(SOUND_CLICK); |
| STOP; // konec couvani |
| FL; FR; // rovne pres diru |
| Wait(PRES_DIRU); |
| STOP; |
| cikcak(); // najdi caru |
| FR; FL; // vpred |
| } // dira |
| |
| } // while(true) |
| } |
| |
| void objizdka() |
| { |
| BL; BR; // zacouvej |
| Wait(15); |
| STOP; |
| |
| cikcak(); // vyrovnej se na caru |
| cikcak(); // radeji 3x, aby se vyrovnaly setrvacne sily |
| cikcak(); |
| |
| FL; BR; Wait(23); // vpravo |
| STOP; |
| FL; FR; Wait(65); // rovne |
| STOP; |
| BL; FR; Wait(23); // vlevo |
| STOP; |
| FL; FR; Wait(75); // rovne |
| STOP; |
| BL; FR; Wait(23); // vlevo |
| STOP; |
| FL; FR; |
| Wait(20); // kousek rovne |
| // rovne, dokud nenarazis na caru |
| while(THRESHOLD > LSENSOR); // levej senzor, protoze cihla je levo |
| STOP; |
| FL; BR; // toc se doprava, dokud nenarazis na caru |
| while(THRESHOLD > RSENSOR); // pravej senzor, protoze cara je urcite vpravo |
| STOP; |
| line=L; // caru predpokladame vlevo, kdybysme ji prejeli |
| dira=0; |
| FL; FR; // vpred |
| } |