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; |
} |
} |
|
|