Diff: Processes/AI/ai.cpp

Revision:

55:0c8897da6b3a

Parent:

50:937e860f4621

Parent:

54:99d3158c9207

Child:

56:ed585a82092b

--- a/Processes/AI/ai.cpp	Fri Apr 12 21:34:34 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 22:00:32 2013 +0000
@@ -1,44 +1,46 @@
 #include "ai.h"
+#include "rtos.h"
+#include "globals.h"
+#include "motion.h"
+#include "Colour.h"
+#include "supportfuncs.h"
+
 
 namespace AI
 {
 
 void ailayer(void const *dummy)
 {
-    Waypoint *current_waypoint = new Waypoint[5];
+    Waypoint current_waypoint;
 
-    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.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;
 
-    motion::setNewWaypoint(current_waypoint);
-    
-    int currwptidx = 0;
+    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)
+
+    float r = 0.61+0.02;
+
+    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 (motion::checkWaypointStatus() && c_upper.getColour()==RED)
         {
-            if(c_upper.getColour() == BLUE && c_lower.getColour() == RED){
-                motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
-                motion::clearWaypointReached();
-            }
-
+            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);
     }
 }