Diff: Processes/AI/ai.cpp

Revision:

46:adcd57a5e402

Parent:

40:44d3dea4adcc

--- a/Processes/AI/ai.cpp	Fri Apr 12 20:40:52 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 20:59:18 2013 +0000
@@ -23,18 +23,24 @@
 */
 
     current_waypoint[0].x = 0.5;
-    current_waypoint[0].y = 1.85;
+    current_waypoint[0].y = 1.65;
     current_waypoint[0].theta = 0.0;
     current_waypoint[0].pos_threshold = 0.05;
     current_waypoint[0].angle_threshold = 0.05*PI;
+    
+    current_waypoint[1].x = 2.5;
+    current_waypoint[1].y = 0.45;
+    current_waypoint[1].theta = 0.0;
+    current_waypoint[1].pos_threshold = 0.05;
+    current_waypoint[1].angle_threshold = 0.05*PI;    
 
-    current_waypoint[1].x = 1.2;
-    current_waypoint[1].y = 0.18;
-    current_waypoint[1].theta = 0;
-    current_waypoint[1].pos_threshold = 0.01;
-    current_waypoint[1].angle_threshold = 0.00001;
+    current_waypoint[2].x = 1.2;
+    current_waypoint[2].y = 0.18;
+    current_waypoint[2].theta = 0;
+    current_waypoint[2].pos_threshold = 0.01;
+    current_waypoint[2].angle_threshold = 0.00001;
 
-    current_waypoint[2].x = -999;
+    current_waypoint[3].x = -999;
 /*
     //TODO: temp current waypoint hack
     current_waypoint = new Waypoint;
@@ -52,15 +58,19 @@
     secondwp->angle_threshold = 0.00001;
 */    
     motion::setNewWaypoint(current_waypoint);
+        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);
     while(1)
     {
+
         Thread::wait(50);
         
+        
         motion::waypoint_flag_mutex.lock();
         if (motion::checkWaypointStatus())
         {
             
-            if ((current_waypoint+1)->x != -999)
+            if (((current_waypoint+1)->x != -999) && ((c_upper.getColour()==BLUE) || (c_lower.getColour()==RED)))
             { 
                 motion::clearWaypointReached();
                 current_waypoint++;