Diff: Processes/AI/ai.cpp

Revision:

49:665bdca0f2cd

Parent:

39:44d3dea4adcc

Child:

50:937e860f4621

--- a/Processes/AI/ai.cpp	Fri Apr 12 17:03:53 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 21:07:00 2013 +0000
@@ -23,16 +23,16 @@
 */
 
     current_waypoint[0].x = 0.5;
-    current_waypoint[0].y = 1.85;
+    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 = 1.2;
-    current_waypoint[1].y = 0.18;
+    current_waypoint[1].x = 2.5;
+    current_waypoint[1].y = 0.5;
     current_waypoint[1].theta = 0;
-    current_waypoint[1].pos_threshold = 0.01;
-    current_waypoint[1].angle_threshold = 0.00001;
+    current_waypoint[1].pos_threshold = 0.05;
+    current_waypoint[1].angle_threshold = 0.05*PI;
 
     current_waypoint[2].x = -999;
 /*
@@ -52,6 +52,8 @@
     secondwp->angle_threshold = 0.00001;
 */    
     motion::setNewWaypoint(current_waypoint);
+    
+    int currwptidx = 0;
     while(1)
     {
         Thread::wait(50);
@@ -60,12 +62,8 @@
         if (motion::checkWaypointStatus())
         {
             
-            if ((current_waypoint+1)->x != -999)
-            { 
-                motion::clearWaypointReached();
-                current_waypoint++;
-                motion::setNewWaypoint(current_waypoint);
-            }
+            motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
+            motion::clearWaypointReached();
         }
         motion::waypoint_flag_mutex.unlock();
     }