Diff: Processes/AI/ai.cpp

Revision:

69:4b7bb92916da

Parent:

67:be3ea5450cc7

Parent:

68:662164480f60

Child:

70:0da6ca845762

--- a/Processes/AI/ai.cpp	Sun Apr 14 18:47:17 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 20:23:07 2013 +0000
@@ -6,39 +6,55 @@
 #include "supportfuncs.h"
 #include "Arm.h"
 
+//TODO: after 2013, kill entire AI layer as it is hacked together. Rest is fine-ish
 
 namespace AI
 {
 
+bool delayed_done = true; //TODO: kill
+
+struct delayed_struct //TODO: kill
+{
+    osThreadId tid; 
+    Waypoint *wpptr;
+};
+
+void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
+
 void ailayer(void const *dummy)
 {
     Waypoint 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);
 
+    // first waypoint for approach
     current_waypoint.x = 2.2;
     current_waypoint.y = 1.85;
     current_waypoint.theta = (-3.0f/4.0f)*PI;
     current_waypoint.pos_threshold = 0.03;
     current_waypoint.angle_threshold = 0.02*PI;
-
-    motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
+    
     motion::collavoiden = 1;
-
-    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);
+    motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
 
     float r = 0.61+0.02+0.01;
 
     bool firstavoidstop = 1;
+    delayed_struct ds = {Thread::gettid(),&current_waypoint};
+    
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop))
+        if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop) && delayed_done)
         {
             //temphack!!!!!
-            Thread::wait(1000);
+            //Thread::wait(1000);
             arm::upper_arm.go_down();
-            Thread::wait(2000);
-            arm::upper_arm.go_up();
+            delayed_done = false;
+            RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
+            //Thread::wait(2000);
+            //arm::upper_arm.go_up();
             
             phi -= 22.5/180*PI;
             current_waypoint.x = 1.5-r*cos(phi);
@@ -52,8 +68,9 @@
             current_waypoint.pos_threshold = 0.01;
             current_waypoint.angle_threshold = 0.01*PI;
 
-            motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
-            if (firstavoidstop){
+            //motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
+            if (firstavoidstop)
+            {
                 motion::collavoiden = 0;
                 firstavoidstop = 0;
             }
@@ -66,4 +83,12 @@
     }
 }
 
+void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
+{
+    delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
+    arm::upper_arm.go_up();
+    motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
+    delayed_done = true;
+}
+
 } //namespace
\ No newline at end of file