Revision 69:4b7bb92916da, committed 2013-04-14

Comitter:

rsavitski

Date:

Sun Apr 14 20:23:07 2013 +0000

Parent:

67:be3ea5450cc7

Parent:

68:662164480f60

Child:

70:0da6ca845762

Commit message:

test ai aux hack

Changed in this revision

--- 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

--- a/Processes/Motion/motion.cpp	Sun Apr 14 18:47:17 2013 +0000
+++ b/Processes/Motion/motion.cpp	Sun Apr 14 20:23:07 2013 +0000
@@ -1,8 +1,7 @@
 ////////////////////////////////////////////////////////////////////////////////
 // Motion control unit
 ////////////////////////////////////////////////////////////////////////////////
-// Takes current state of the robot and target waypoint,
-// calculates desired forward and angular velocities and requests those from the motor control layer.
+// Takes current state and motion command (set via accessors) and actuates it via motor layer calls
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
@@ -154,12 +153,16 @@
         
     //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
     
-    if(collavoiden){
+    //TODO: remove oskar's avoidance hack
+    if(collavoiden)
+    {
         float d = ADS.Distanceincm();
-        if(d > 10){
+        if(d > 10)
+        {
             forward_v *= max(min((d-15)*(1.0f/20.0f),1.0f),-0.1f);
         }
     }
+    // end of Oskar hack
     
     // pass values to the motor control
     MotorControl::set_fwdcmd(forward_v);

--- a/Processes/Motion/motion.h	Sun Apr 14 18:47:17 2013 +0000
+++ b/Processes/Motion/motion.h	Sun Apr 14 20:23:07 2013 +0000
@@ -12,11 +12,7 @@
 void motionlayer(void const *dummy);
 void waypoint_motion_handler();
 
-// TODO: privatise (via unnamed namespace) non-API parts
 void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp);
-
-
-
 bool checkMotionStatus();