Revision 78:3178a1e46146, committed 2013-04-15

Comitter:

rsavitski

Date:

Mon Apr 15 17:07:40 2013 +0000

Parent:

75:283283604485

Parent:

77:8d83a0c00e66

Child:

80:b9b5d5ecea71

Child:

81:ef1ce4f5322b

Commit message:

merged x2

Changed in this revision

--- a/Processes/AI/ai.cpp	Mon Apr 15 15:29:40 2013 +0000
+++ b/Processes/AI/ai.cpp	Mon Apr 15 17:07:40 2013 +0000
@@ -7,10 +7,25 @@
 #include "Arm.h"
 #include "MotorControl.h"
 
-//TODO: after 2013, kill entire AI layer as it is hacked together. Rest is fine-ish
+//TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
+
+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);
 
 namespace AI
 {
+// starting position
+#ifdef TEAM_RED
+ColourEnum own_colour = RED;
+
+Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32};
+#endif
+
+#ifdef TEAM_BLUE
+ColourEnum own_colour = BLUE;
+
+Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
+#endif
 
 bool delayed_done = true; //TODO: kill
 
@@ -20,16 +35,57 @@
     Waypoint *wpptr;
 };
 
-void arm_upper(const void *dummy); //TODO: kill
+void raise_top_arm(const void *dummy);
+void raise_bottom_arm(const void *dummy);
 
 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
 
 void ailayer(void const *dummy)
 {
-    Waypoint current_waypoint;
+    RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
+    RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
+    
+    //NB: reassign ds.wpptr before each invocation
+    delayed_struct ds = {Thread::gettid(),NULL};
+    RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
+
+    MotorControl::motorsenabled = true; 
+    motion::collavoiden = 1;
+    
+    motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
+    Thread::signal_wait(0x1); //wait until wp reached
+   
+    MotorControl::motorsenabled = false;
+    
+    Thread::signal_wait(0x2); //wait for cord
+    
+    // CORD PULLED
+    
+    Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
     
-    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);
+    MotorControl::motorsenabled = true;
+    motion::setNewWaypoint(Thread::gettid(),&mid_wp);
+    Thread::signal_wait(0x1); //wait until wp reached
+    
+    Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
+    motion::setNewWaypoint(Thread::gettid(),&approach_wp); 
+    Thread::signal_wait(0x1); //wait until wp reached
+    
+    // TODO: ?(disable collison avoidance), approach first cake wp, enable collision
+    /*
+    ///////////////temp hack
+    Waypoint temp_wp = {1.222,1.440, 149.0/180*PI, 0.01, 0.02*PI, 512};
+    motion::setNewWaypoint(Thread::gettid(),&temp_wp); 
+    Thread::signal_wait(0x1); //wait until wp reached
+    arm::lower_arm.go_down();
+    bottom_arm_up_timer.start(1200);
+    /////////////// temp hack over
+    */
+    
+    while(1)
+        Thread::wait(1000);
+    /*
+    ///////////////////////////////////////////////////////
 
     // first waypoint for approach
     current_waypoint.x = 2.2;
@@ -42,23 +98,23 @@
     MotorControl::motorsenabled = 1;
     motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
 
-    float r = 0.61+0.02+0.01;
+    float r = 0.26+0.35+0.02+0.01;
 
     bool firstavoidstop = 1;
     delayed_struct ds = {Thread::gettid(),&current_waypoint};
     RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
-    RtosTimer delayed_armer(arm_upper, osTimerOnce);
+    RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
     
     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) && delayed_done)
+        if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done)
         {
             //temphack!!!!!
             //Thread::wait(1000);
             arm::upper_arm.go_down();
             delayed_done = false;
-            delayed_armer.start(1200);
+            top_arm_up_timer.start(1200);
             delayed_wp_set.start(2400);
             //Thread::wait(2000);
             //arm::upper_arm.go_up();
@@ -87,18 +143,22 @@
         motion::waypoint_flag_mutex.unlock();            
         
         Thread::wait(50);
-    }
+    }*/
 }
 
-void arm_upper(const void *dummy)
+void raise_top_arm(const void *dummy)
 {
     arm::upper_arm.go_up();
 }
 
+void raise_bottom_arm(const void *dummy)
+{
+    arm::lower_arm.go_up();
+}
+
 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;
 }

--- a/Processes/Motion/motion.cpp	Mon Apr 15 15:29:40 2013 +0000
+++ b/Processes/Motion/motion.cpp	Mon Apr 15 17:07:40 2013 +0000
@@ -44,7 +44,7 @@
 };
 
 // local copy of the current active motion
-motion_cmd current_motion;
+motion_cmd current_motion = {motion_waypoint, 0, false, NULL};
 
 Waypoint target_waypoint = {0,0,0,0,0}; //local wp copy, TODO: fix and make a shared local memory pool for any movement cmd to be copied to
 
@@ -132,10 +132,10 @@
     
     
     // forward velocity controller
-    const float p_gain_fv = 1;//0.7; //TODO: tune
+    const float p_gain_fv = 0.9;//0.7; //TODO: tune
     
     float max_fv = 0.3;//0.2; // meters per sec //TODO: tune
-    float max_fv_reverse = 0.05; //TODO: tune
+    float max_fv_reverse = 0.03; //TODO: tune
     const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune
     
     // control, distance_err in meters
@@ -146,7 +146,7 @@
         max_fv = max_fv_reverse;
     
     // control the forward velocity envelope based on angular error
-    max_fv = max_fv * pow(cos(angle_err_saved/2), angle_envelope_exponent);
+    max_fv = max_fv * pow(cos(angle_err_saved/2), /*angle_envelope_exponent*/target_waypoint.angle_exponent); //temp hack
     
     // constrain range
     if (forward_v > max_fv)
@@ -186,7 +186,6 @@
     current_motion.setter_tid = setter_tid_in;
     current_motion.motion_type = motion_waypoint;
 
-    //current_motion.wp_ptr = new_wp; //TODO: need to make local copy or edits to mem ptr contents screw motion over
     target_waypoint = *new_wp;
 }
 

--- a/Processes/Motion/motion.h	Mon Apr 15 15:29:40 2013 +0000
+++ b/Processes/Motion/motion.h	Mon Apr 15 17:07:40 2013 +0000
@@ -12,6 +12,7 @@
 void motionlayer(void const *dummy);
 void waypoint_motion_handler();
 
+// supports both polling on sticky bit via checkMotionStatus(), waiting/blocking on signal 0x1 in setter thread (signalled upon reaching the wp)
 void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp);
 bool checkMotionStatus();
 

--- a/globals.h	Mon Apr 15 15:29:40 2013 +0000
+++ b/globals.h	Mon Apr 15 17:07:40 2013 +0000
@@ -83,7 +83,7 @@
 const PinName P_COLOR_SENSOR_RED_LOWER = p11;
 const PinName P_COLOR_SENSOR_BLUE_LOWER = p10;
 
-const PinName P_START_CORD = p27;
+const PinName P_START_CORD = p17;
 
 
 
@@ -112,6 +112,7 @@
     float theta;
     float pos_threshold;
     float angle_threshold;
+    float angle_exponent; //temp hack
 } Waypoint;
 
 typedef struct State