| 0,0 → 1,118 |
|---|
| #define THRESHOLD 44 // rozhodovaci uroven mezi cernou a bilou |
| #define FTHRESHOLD 150 // Rozhodovaci uroven dalkoveho sensoru |
| #define RED_WHITE 60 // maximum bile na cervenem senzoru |
| #define MLEFT OUT_C |
| #define MRIGHT OUT_A |
| #define RSENSOR SENSOR_1 |
| #define LSENSOR SENSOR_3 |
| #define BUMPER SENSOR_2 |
| |
| #define L 0 |
| #define R 1 |
| #define S 2 |
| |
| int line; |
| |
| 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_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW)); // senzor na prekazku |
| SetTxPower(TX_POWER_HI); // aby daleko videl |
| |
| // start cihla; |
| // start cara; |
| SetPower (MLEFT,OUT_HALF); |
| SetPower (MRIGHT,OUT_HALF); |
| OnRev(MLEFT); // rozjed se vpred |
| OnFwd(MRIGHT); |
| |
| line=S; |
| |
| while(true) |
| { |
| if (THRESHOLD < RSENSOR) |
| { |
| line=R; |
| SetPower (MRIGHT,OUT_LOW); |
| OnRev(MLEFT); |
| }; |
| if (THRESHOLD < LSENSOR) |
| { |
| line=L; |
| SetPower (MLEFT,OUT_LOW); |
| OnFwd(MRIGHT); |
| }; |
| |
| if ((THRESHOLD > LSENSOR)&&(THRESHOLD > RSENSOR)) |
| { |
| if(L==line) |
| { |
| Off(MLEFT); |
| OnFwd(MRIGHT); |
| while(THRESHOLD > LSENSOR); |
| while(THRESHOLD < LSENSOR); |
| }; |
| if(R==line) |
| { |
| Off(MRIGHT); |
| OnRev(MLEFT); |
| while(THRESHOLD > RSENSOR); |
| while(THRESHOLD < RSENSOR); |
| }; |
| line=S; |
| SetPower (MLEFT,OUT_HALF); |
| SetPower (MRIGHT,OUT_HALF); |
| OnRev(MLEFT); // rozjed se vpred |
| OnFwd(MRIGHT); |
| } |
| } |
| } |
| |
| task cara() |
| { |
| while(true) |
| { |
| OnRev(MLEFT); // rozjed se vpred |
| OnFwd(MRIGHT); |
| if (THRESHOLD < RSENSOR) |
| { |
| OnRev(MRIGHT); |
| Wait(1); |
| Off(MRIGHT); |
| }; |
| if (THRESHOLD < LSENSOR) |
| { |
| OnFwd(MLEFT); |
| Wait(1); |
| Off(MLEFT); |
| }; |
| while((THRESHOLD < RSENSOR)||(THRESHOLD < LSENSOR)); |
| } |
| } |
| |
| 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(100); |
| start cara; |
| } |
| lastlevel = BUMPER; |
| lastlevel -= FTHRESHOLD; |
| } |
| } |
| |
| |