Diff: Processes/AI/ai.cpp

Revision:

50:937e860f4621

Parent:

49:665bdca0f2cd

Parent:

46:adcd57a5e402

Child:

55:0c8897da6b3a

--- a/Processes/AI/ai.cpp	Fri Apr 12 21:07:00 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 21:26:02 2013 +0000
@@ -6,54 +6,25 @@
 void ailayer(void const *dummy)
 {
     Waypoint *current_waypoint = new Waypoint[5];
-/*
-    current_waypoint[0].x = 1;
-    current_waypoint[0].y = 1;
-    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.2;
-    current_waypoint[1].y = 1.5;
-    current_waypoint[1].theta = PI/2;
-    current_waypoint[1].pos_threshold = 0.05;
-    current_waypoint[1].angle_threshold = 0.05*PI;
-
-    current_waypoint[2].x = -999;
-*/
 
     current_waypoint[0].x = 0.5;
     current_waypoint[0].y = 0.5;
     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.5;
     current_waypoint[1].theta = 0;
     current_waypoint[1].pos_threshold = 0.05;
     current_waypoint[1].angle_threshold = 0.05*PI;
 
-    current_waypoint[2].x = -999;
-/*
-    //TODO: temp current waypoint hack
-    current_waypoint = new Waypoint;
-    current_waypoint->x = 0.5;
-    current_waypoint->y = 0.7;
-    current_waypoint->theta = 0.0;
-    current_waypoint->pos_threshold = 0.05;
-    current_waypoint->angle_threshold = 0.05*PI;
-    
-    Waypoint* secondwp = new Waypoint;
-    secondwp->x = 1.20;
-    secondwp->y = 0.18;
-    secondwp->theta = PI;
-    secondwp->pos_threshold = 0.01;
-    secondwp->angle_threshold = 0.00001;
-*/    
     motion::setNewWaypoint(current_waypoint);
     
     int currwptidx = 0;
+    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);
@@ -61,9 +32,11 @@
         motion::waypoint_flag_mutex.lock();
         if (motion::checkWaypointStatus())
         {
-            
-            motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
-            motion::clearWaypointReached();
+            if(c_upper.getColour() == BLUE && c_lower.getColour() == RED){
+                motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
+                motion::clearWaypointReached();
+            }
+
         }
         motion::waypoint_flag_mutex.unlock();
     }