Obstacle avoidance demo tuned for Embeddedworld

diff --git a/navratil_ad/navratil_ad.c b/navratil_ad/navratil_ad.c
index b8c4749810c00a60178fa64521d9f7d9d6aa02e6..6cb5c70415e8481ecf9983d6728ff749f48e160b 100644 (file)
--- a/navratil_ad/navratil_ad.c
+++ b/navratil_ad/navratil_ad.c
@@ -16,6 +16,7 @@
 #include <system_def.h>
 #include <stdlib.h>
 #include <string.h>
+#include <stdbool.h>
 
 #include <pxmc.h>
 #include <pxmcbsp.h>
@@ -166,6 +167,12 @@ init(void)
 #define MAX_R 300 /* mm */
 #define MIN_R (WHEEL_DIST/2) /* mm */
 
+void move2(int speedl, int speedr)
+{
+  pxmc_spd(&mcsX0, +speedl, 0);
+  pxmc_spd(&mcsX1, -speedr - 10, 0); // minus 100 je kompenzace zataceni
+}
+
 void move(int speed, int r)
 {
   int sl, sr;
@@ -177,24 +184,28 @@ void move(int speed, int r)
     sl = speed - (long long)speed * (d/2) / r;
   } else {
     sr = speed;
-    sl = speed + 10;           // minus 100 je kompenzace zataceni
+    sl = speed;
   }
-  pxmc_spd(&mcsX0, +sl, 0);
-  pxmc_spd(&mcsX1, -sr, 0);
+  move2(sl, sr);
   //printf("speed=%5d, r=%5d, sl=%5d, sr=%5d\n", speed, r, sl, sr);
 }
 
+
+
 #define DIST_TRESHOLD 280
+#define BACKWARD_TIMEOUT 4 /* sec */
 
 void autonomni_pohyb(void)
 {
   long int now = get_timer();
-  long int next = now;
-  long int straight_timout = 0;
+  long int turn_timeout = now;
+  long int backward_timeout = now + 10*BACKWARD_TIMEOUT;
   int speed = 3000;
-  int radius = 70;
+  int radius = 50;
   enum { TURN, STRAIGHT } stav = TURN;                         // podle priznaku stavu se rozhoduje,zda pojede rovne, nebo zatoci
   uint16_t voltage1, voltage2;
+  bool obst1=false, obst2=false;
+  bool old_obst1, old_obst2;
 
   while (1) {
     
@@ -203,38 +214,50 @@ void autonomni_pohyb(void)
     voltage1 = read_GP2();
     voltage2 = read_GP1();
 
-    if (voltage1 >= DIST_TRESHOLD) DEB_LED_ON(1);
+    old_obst1 = obst1;
+    obst1 = (voltage1 >= DIST_TRESHOLD);
+    if (obst1) DEB_LED_ON(1);
     else DEB_LED_OFF(1);
 
-    if (voltage2 >= DIST_TRESHOLD) DEB_LED_ON(2);
+    old_obst2 = obst2;
+    obst2 = (voltage2 >= DIST_TRESHOLD);
+    if (obst2) DEB_LED_ON(2);
     else DEB_LED_OFF(2);
-    
-    if (stav == TURN && now >= next){                          // podminka pro jizdu 
+
+    if (obst1 != old_obst1 ||
+       obst2 != old_obst2) {
+      backward_timeout = now + BACKWARD_TIMEOUT/*sec*/*10;
+    }
+
+    /* Test for long interval without sensor change */
+    if (now - backward_timeout >= 0) {
+      //Go backward
+      speed = -speed;
+      stav = STRAIGHT;
+      move(speed,0);
+      backward_timeout = now + BACKWARD_TIMEOUT/*sec*/*10;
+    }
+
+    /* Test for end of turn */
+    if (stav == TURN && now - turn_timeout >= 0) {
       stav = STRAIGHT;
-      straight_timout = 10/*sec*/*10 + now;
       move(speed,0);
     }
 
-    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)
     }
-  }                                                            //konec while
-}                                                              //konec funkce
+  }
+}
 
 
 void sleduj_caru(void){