Diff: Processes/AI/ai.cpp

Revision:

54:99d3158c9207

Parent:

53:b013df99b747

Child:

55:0c8897da6b3a

--- a/Processes/AI/ai.cpp	Fri Apr 12 18:22:53 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 20:58:59 2013 +0000
@@ -5,70 +5,35 @@
 
 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;
+    Waypoint current_waypoint;
 
-    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 = 1.8;
-    current_waypoint[0].y = 1.39;
-    current_waypoint[0].theta = PI;
-    current_waypoint[0].pos_threshold = 0.03;
-    current_waypoint[0].angle_threshold = 0.05*PI;
+    current_waypoint.x = 2.2;
+    current_waypoint.y = 1.85;
+    current_waypoint.theta = PI;
+    current_waypoint.pos_threshold = 0.01;
+    current_waypoint.angle_threshold = 0.02*PI;
 
-    current_waypoint[1].x = 1.5;
-    current_waypoint[1].y = 1.39;
-    current_waypoint[1].theta = PI;
-    current_waypoint[1].pos_threshold = 0.03;
-    current_waypoint[1].angle_threshold = 0.05*PI;
+    motion::setNewWaypoint(&current_waypoint);
+    
 
-    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;
-*/    
+    float r = 0.61;
 
-    motion::setNewWaypoint(current_waypoint);
-    while(1)
+    for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
-        Thread::wait(50);
-        
         motion::waypoint_flag_mutex.lock();
         if (motion::checkWaypointStatus())
         {
-            
-            if ((current_waypoint+1)->x != -999)
-            { 
-                motion::clearWaypointReached();
-                current_waypoint++;
-                motion::setNewWaypoint(current_waypoint);
-            }
+            phi -= 22.5/180*PI;
+            current_waypoint.x = 1.5-r*cos(phi);
+            current_waypoint.y = 2-r*sin(phi);
+            current_waypoint.theta = constrainAngle(phi+PI/2);
+            motion::clearWaypointReached();
+            motion::setNewWaypoint(&current_waypoint);
         }
-        motion::waypoint_flag_mutex.unlock();
+        motion::waypoint_flag_mutex.unlock();            
+        
+        Thread::wait(50);
     }
 }