| 0,0 → 1,153 |
|---|
| #define THRESHOLD 44 // rozhodovaci uroven mezi cernou a bilou |
| #define FTHRESHOLD 100 // Rozhodovaci uroven dalkoveho sensoru |
| #define MLEFT OUT_C // Motory |
| #define MRIGHT OUT_A |
| #define RSENSOR SENSOR_1 // Sensory na caru |
| #define LSENSOR SENSOR_3 |
| #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 |
| int derivace; // jak moc se bude zachovavat predesly smer |
| |
| void Straight() // rovne |
| { |
| SetPower (MLEFT,OUT_FULL); |
| SetPower (MRIGHT,OUT_FULL); |
| OnRev(MLEFT); |
| OnFwd(MRIGHT); |
| derivace=5; |
| movement=S; |
| } |
| |
| void Left() // doleva |
| { |
| SetPower (MLEFT,OUT_LOW); |
| SetPower (MRIGHT,OUT_FULL); |
| Off(MLEFT); |
| OnFwd(MRIGHT); |
| derivace=3; |
| movement=L; |
| } |
| |
| void Right() // doprava |
| { |
| SetPower (MLEFT,OUT_FULL); |
| SetPower (MRIGHT,OUT_LOW); |
| OnRev(MLEFT); |
| Off(MRIGHT); |
| derivace=3; |
| 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 |
| |
| movement=S; |
| line=S; |
| dira=0; |
| |
| // start cara; |
| start cihla; |
| |
| Straight(); |
| while(true) |
| { |
| if (THRESHOLD < RSENSOR) |
| { |
| line=R; |
| dira=0; |
| Right(); |
| if (THRESHOLD < RSENSOR) |
| { |
| while(THRESHOLD < RSENSOR); |
| } |
| else |
| |
| }; |
| if (THRESHOLD < LSENSOR) |
| { |
| if (L==movement) |
| { |
| Straight(); |
| } |
| else |
| { |
| line=L; |
| dira=0; |
| Left(); |
| }; |
| }; |
| } |
| } |
| |
| |
| task cara() |
| { |
| Left(); |
| while(true) |
| { |
| if (8 < dira++) |
| { |
| PlaySound(SOUND_DOUBLE_BEEP); |
| if (L==line) line=R; else line=L; |
| Off(MLEFT); |
| Off(MRIGHT); |
| Wait(500); |
| dira=0; |
| }; |
| |
| switch(line) |
| { |
| case S: |
| break; |
| case L: |
| Left(); |
| break; |
| |
| case R: |
| Right(); |
| break; |
| }; |
| // line=S; |
| Wait(5); |
| Straight(); |
| Wait(5); |
| } |
| } |
| |
| task cihla() // dalkovy sensor |
| { |
| int lastlevel; |
| |
| lastlevel = 0; |
| while(true) |
| { |
| SendMessage(0); |
| if(lastlevel > BUMPER) |
| { |
| PlaySound(SOUND_FAST_UP); |
| stop cara; |
| Off(MLEFT); |
| Off(MRIGHT); |
| Wait(300); |
| start cara; |
| } |
| lastlevel = BUMPER; |
| lastlevel -= FTHRESHOLD; |
| } |
| } |
| |
| |