Diff: Processes/AI/ai.cpp

Revision:

66:f1d75e51398d

Parent:

57:d434ceab6892

Child:

67:be3ea5450cc7

diff -r c979fb1cd3b5 -r f1d75e51398d Processes/AI/ai.cpp
--- a/Processes/AI/ai.cpp	Sun Apr 14 14:59:57 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 17:22:20 2013 +0000
@@ -20,16 +20,18 @@
     current_waypoint.angle_threshold = 0.02*PI;
 
     motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
+    motion::collavoiden = 1;
 
     Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
     Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
 
     float r = 0.61+0.02;
 
+    bool firstavoidstop = 1;
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkMotionStatus() && c_lower.getColour()==RED)
+        if (motion::checkMotionStatus() && (c_lower.getColour()==RED || firstavoidstop))
         {
             phi -= 22.5/180*PI;
             current_waypoint.x = 1.5-r*cos(phi);
@@ -41,6 +43,12 @@
             current_waypoint.y += 0.0425*sin(current_waypoint.theta);
 
             motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
+            if (firstavoidstop){
+                motion::collavoiden = 0;
+                firstavoidstop = 0;
+            }
+            else
+                motion::collavoiden = 1;
         }
         motion::waypoint_flag_mutex.unlock();