| 0,0 → 1,154 |
|---|
| #define THRESHOLD 44 // rozhodovaci uroven mezi cernou a bilou |
| #define FTHRESHOLD 100 // Rozhodovaci uroven dalkoveho sensoru |
| #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 // Sensory na caru |
| #define LSENSOR SENSOR_1 |
| #define BUMPER SENSOR_2 // Dalkovy sensor |
| |
| #define L 0 // left |
| #define R 1 // right |
| #define S 2 // straight |
| |
| int movement; // smer minuleho pohybu |
| int line; // na ktere strane byla detekovana cara |
| int dira; // pocitadlo pro nalezeni preruseni cary |
| |
| void Straight() // rovne |
| { |
| FL; |
| FR; |
| movement=S; |
| } |
| |
| void Left() // doleva |
| { |
| Off(ML); |
| FR; |
| movement=L; |
| } |
| |
| void Right() // doprava |
| { |
| FL; |
| Off(MR); |
| movement=R; |
| } |
| |
| 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 |
| // sensor na prekazku |
| SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW)); |
| SetTxPower(TX_POWER_HI); // aby daleko videl |
| |
| SetPower (ML,OUT_FULL); // vykon motoru |
| SetPower (MR,OUT_FULL); |
| movement=S; |
| line=S; |
| dira=0; |
| |
| start cara; |
| start cihla; |
| } |
| |
| task cara() // sledovani cary |
| { |
| Straight(); |
| while(true) |
| { |
| if (THRESHOLD < RSENSOR) // Cara vpravo |
| { |
| line=R; // cara zrejme bude na druhe strane |
| dira=0; // nuluj pocitadlo diry |
| switch(movement) |
| { |
| case L: |
| Right(); |
| break; |
| default: |
| Straight(); |
| }; |
| } |
| else |
| if (THRESHOLD < LSENSOR) // Cara vlevo |
| { |
| line=L; // zaznamenej, kdes videl caru |
| dira=0; // nuluj pocitadlo diry |
| switch(movement) |
| { |
| case R: |
| Left(); |
| break; |
| default: |
| Straight(); |
| }; |
| } |
| else // sensory mimo caru |
| { |
| if (20 < dira++) // nejedeme uz moc dlouho bez cary? |
| { |
| PlaySound (SOUND_FAST_UP); |
| switch(movement) |
| { |
| case L: |
| line=R; // cara zrejme bude na druhe strane |
| Off(ML); //couvej |
| BR; |
| break; |
| case R: |
| line=L; // cara zrejme bude na druhe strane |
| BL; // couvej |
| Off(MR); |
| break; |
| }; |
| Wait(50); // zpet, ale ne uplne a kousek rovne |
| Straight(); |
| Wait(5); |
| dira=0; // myslime si, ze diru jsme vyresili |
| }; |
| |
| switch(line) // kdyz nevidis caru, tak jed tam, kdes ji videl naposled |
| { |
| case L: |
| Left(); |
| break; |
| case R: |
| Right(); |
| break; |
| }; |
| } |
| } // while(true) |
| } |
| |
| |
| task cihla() // dalkovy sensor |
| { |
| int lastlevel; |
| |
| lastlevel = 0; |
| while(true) |
| { |
| SendMessage(0); |
| if(lastlevel > BUMPER) |
| { |
| PlaySound(SOUND_DOWN); |
| stop cara; |
| Off(ML); // zatim pouze zastav |
| Off(MR); |
| Wait(300); |
| start cara; |
| } |
| lastlevel = BUMPER; |
| lastlevel -= FTHRESHOLD; |
| } |
| } |
| |
| |