From a5148d969f6f90ec5c2b2f4f4a48f84375ca28fc Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Thu, 26 Feb 2009 19:56:50 +0100
Subject: [PATCH] Obstacle avoidance demo tuned for Embeddedworld

---
 navratil_ad/navratil_ad.c | 83 +++++++++++++++++++++++++--------------
 1 file changed, 53 insertions(+), 30 deletions(-)

diff --git a/navratil_ad/navratil_ad.c b/navratil_ad/navratil_ad.c
index b8c4749..6cb5c70 100644
--- 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){
-- 
2.39.2