0,0 → 1,139 |
#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=5; |
movement=L; |
} |
|
void Right() // doprava |
{ |
SetPower (MLEFT,OUT_FULL); |
SetPower (MRIGHT,OUT_LOW); |
OnRev(MLEFT); |
Off(MRIGHT); |
derivace=5; |
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; |
|
while(true) |
{ |
if (THRESHOLD < RSENSOR) {line=R; dira=0;}; |
if (THRESHOLD < LSENSOR) {line=L; dira=0;}; |
} |
} |
|
|
task cara() |
{ |
Left(); |
while(true) |
{ |
switch(line) |
{ |
case S: |
if (3 < dira++) |
{ |
PlaySound(SOUND_DOUBLE_BEEP); |
}; |
break; |
case L: |
switch(movement) |
{ |
// case S: |
case R: |
Left(); |
break; |
case L: |
Right(); |
break; |
}; |
break; |
|
case R: |
switch(movement) |
{ |
// case S: |
case L: |
Right(); |
break; |
case R: |
Left(); |
break; |
}; |
break; |
}; |
line=S; |
Wait(10); |
} |
} |
|
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; |
} |
} |
|
|