- if (stav == STRAIGHT) {
- if (now >= straight_timout) {
- //Go backward
- speed = -speed;
- move(speed,0); //otoc normalne je pouzit o if cyklus drive, ted si ho jen pujcuji
- }
- if ((voltage1 >= DIST_TRESHOLD && speed > 0) ||
- (voltage2 >= DIST_TRESHOLD && speed < 0)) { // podminka pro zatoceni 260 odpovida (mensi nez 20cm)
- //Obstacle detected
- if (rand() % 100 < 50) // 50 % predpoklad pro zatoceni doprava
- move(speed,radius); // zatoc rychlosti speed a polomerem radius(doprava)
- else
- move(speed,-radius); // zatoc rychlosti speed a polomerem -radius (doleva)
- stav = TURN;
-
- next = now + ((rand() % 5)+8); // zataceni po dobu 5 - 12 (nahodne cislo)
- } // konec if
+ /* Test for turn */
+ if (stav == STRAIGHT &&
+ ((obst1 && speed > 0) ||
+ (obst2 && speed < 0))) { // podminka pro zatoceni 260 odpovida (mensi nez 20cm)
+ //Obstacle detected
+ if (rand() % 100 < 50) // 50 % predpoklad pro zatoceni doprava
+ move(speed, +radius); // zatoc rychlosti speed a polomerem radius(doprava)
+ else
+ move(speed, -radius); // zatoc rychlosti speed a polomerem -radius (doleva)
+ stav = TURN;
+ turn_timeout = now + 4 + (rand() % 4); // zataceni po urcitou dobu (nahodne cislo)