Diff: Processes/AI/ai.cpp

Revision:

39:44d3dea4adcc

Parent:

38:c9058a401410

Child:

46:adcd57a5e402

Child:

49:665bdca0f2cd

Child:

52:bffe5f7c39a3

--- a/Processes/AI/ai.cpp	Wed Apr 10 22:30:09 2013 +0000
+++ b/Processes/AI/ai.cpp	Thu Apr 11 17:23:07 2013 +0000
@@ -3,16 +3,10 @@
 namespace AI
 {
 
-Mutex waypoint_flag_mutex;
-
-Waypoint* current_waypoint = NULL; //global scope
-
-bool waypoint_reached = false; // is current waypoint reached
-
 void ailayer(void const *dummy)
 {
-    current_waypoint = new Waypoint[5];
-
+    Waypoint *current_waypoint = new Waypoint[5];
+/*
     current_waypoint[0].x = 1;
     current_waypoint[0].y = 1;
     current_waypoint[0].theta = 0.0;
@@ -26,7 +20,21 @@
     current_waypoint[1].angle_threshold = 0.05*PI;
 
     current_waypoint[2].x = -999;
+*/
 
+    current_waypoint[0].x = 0.5;
+    current_waypoint[0].y = 1.85;
+    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].theta = 0;
+    current_waypoint[1].pos_threshold = 0.01;
+    current_waypoint[1].angle_threshold = 0.00001;
+
+    current_waypoint[2].x = -999;
 /*
     //TODO: temp current waypoint hack
     current_waypoint = new Waypoint;
@@ -43,35 +51,24 @@
     secondwp->pos_threshold = 0.01;
     secondwp->angle_threshold = 0.00001;
 */    
-    
+    motion::setNewWaypoint(current_waypoint);
     while(1)
     {
         Thread::wait(50);
         
-        waypoint_flag_mutex.lock();
-        if (checkWaypointStatus())
+        motion::waypoint_flag_mutex.lock();
+        if (motion::checkWaypointStatus())
         {
-            clearWaypointReached();
-            if ((current_waypoint+1)->x != -999) 
+            
+            if ((current_waypoint+1)->x != -999)
+            { 
+                motion::clearWaypointReached();
                 current_waypoint++;
+                motion::setNewWaypoint(current_waypoint);
+            }
         }
-        waypoint_flag_mutex.unlock();
+        motion::waypoint_flag_mutex.unlock();
     }
 }
 
-void setWaypointReached()
-{
-    waypoint_reached = true;
-}
-
-void clearWaypointReached()
-{
-    waypoint_reached = false;
-}
-
-bool checkWaypointStatus()
-{
-    return waypoint_reached;
-}
-
 } //namespace
\ No newline at end of file