Revision 61:a7782a35ce4f, committed 2013-04-14

Comitter:

madcowswe

Date:

Sun Apr 14 10:49:51 2013 +0000

Parent:

60:5058465904e0

Parent:

58:ff2121f34e3b

Child:

62:78d99b781f02

Commit message:

merge..

Changed in this revision

--- a/Processes/AI/ai.cpp	Sat Apr 13 22:41:00 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 10:49:51 2013 +0000
@@ -19,7 +19,7 @@
     current_waypoint.pos_threshold = 0.01;
     current_waypoint.angle_threshold = 0.02*PI;
 
-    motion::setNewWaypoint(&current_waypoint);
+    motion::setNewWaypoint(Thread::gettid(),&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);
@@ -29,14 +29,18 @@
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkWaypointStatus() && c_upper.getColour()==RED)
+        if (motion::checkMotionStatus() && c_lower.getColour()==RED)
         {
             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);
+            
+            //arm offset
+            current_waypoint.x += 0.0425*cos(current_waypoint.theta);
+            current_waypoint.y += 0.0425*sin(current_waypoint.theta);
+
+            motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
         }
         motion::waypoint_flag_mutex.unlock();            
         

--- a/Processes/Motion/motion.cpp	Sat Apr 13 22:41:00 2013 +0000
+++ b/Processes/Motion/motion.cpp	Sun Apr 14 10:49:51 2013 +0000
@@ -6,22 +6,66 @@
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
+#include "math.h"
+#include "Kalman.h"
+#include "MotorControl.h"
+#include "supportfuncs.h"
+
+namespace motion
+{
+    // private function prototypes
+    
+    void setWaypointReached();
+    void clearMotionCmd();
+    void clearWaypoint();
+}
 
 namespace motion
 {
 
+// motion commands supported
+enum motion_type_t { motion_waypoint };
+
+struct motion_cmd
+{
+    motion_type_t motion_type;    
+    osThreadId setter_tid;    
+    
+    bool motion_done;
+    
+    union 
+    {
+        Waypoint *wp_ptr;
+    };
+
+};
+
+// local copy of the current active motion
+motion_cmd current_motion;
+
 Mutex waypoint_flag_mutex;
 
-Waypoint *current_waypoint = NULL; 
+// local to waypoint motion handler
+bool wp_d_reached = false;
+bool wp_a_reached = false;
 
-bool waypoint_reached = false; // is current waypoint reached
-bool d_reached = false;
-bool a_reached = false;
 
 void motionlayer(void const *dummy)
-{    
+{   
+    switch (current_motion.motion_type)
+    {
+        case motion_waypoint:
+            waypoint_motion_handler();
+        break;
+        default:
+        break;
+    }
+}
+
+void waypoint_motion_handler()
+{
     // save target waypoint
-    Waypoint target_waypoint = *current_waypoint;
+    Waypoint target_waypoint = *current_motion.wp_ptr;
     
     // get current state from Kalman
     State current_state = Kalman::getState();
@@ -35,9 +79,9 @@
     
     float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
     
-    // reversing
+    // reversing if close to target and more optimal than forward movement
     bool reversing = false;
-    if ((abs(angle_err) > PI/2) && (distance_err < 0.2))
+    if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) //TODO: parameterise and tune 0.2 meters hardcoded
     {
         reversing = true;
         angle_err = constrainAngle(angle_err + PI);
@@ -47,24 +91,22 @@
     float angle_err_saved = angle_err; // actuated angle error can be overriden by the final turning code, but forward speed envelope should be controlled with the atan angle
     
     // is the waypoint reached
-    waypoint_flag_mutex.lock();
-    if (abs(distance_err) < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
+    waypoint_flag_mutex.lock(); //TODO: consider refactoring
+    if (abs(distance_err) < ((wp_d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
     {
-        d_reached = true;
-        //distance_err = 0;
+        wp_d_reached = true;
         
         angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
         
         if (abs(angle_err) < target_waypoint.angle_threshold)
         {
-            a_reached = true;
-            //angle_err = 0;
+            wp_a_reached = true;
         }
     } 
     waypoint_flag_mutex.unlock();
         
     waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
-    if (d_reached && a_reached)
+    if (wp_d_reached && wp_a_reached)
     {
         setWaypointReached();
     }
@@ -90,7 +132,7 @@
     
     float max_fv = 0.2; // meters per sec //TODO: tune
     float max_fv_reverse = 0.05; //TODO: tune
-    const float angle_envelope_exponent = 512;//32.0; //8.0; //TODO: tune
+    const float angle_envelope_exponent = 32.0;//512; //8.0; //TODO: tune
     
     // control, distance_err in meters
     float forward_v = p_gain_fv * distance_err;
@@ -115,26 +157,52 @@
     MotorControl::set_omegacmd(angular_v);
 }
 
-void setNewWaypoint(Waypoint *new_wp)
+
+bool checkMotionStatus()
+{
+    return current_motion.motion_done;
+}
+
+
+void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp)
 {
-    current_waypoint = new_wp;
+    clearMotionCmd();
+
+    current_motion.setter_tid = setter_tid_in;
+    current_motion.motion_type = motion_waypoint;
+
+    current_motion.wp_ptr = new_wp;
 }
 
+
 void setWaypointReached()
 {
-    waypoint_reached = true;
+    current_motion.motion_done = true;
+    osSignalSet(current_motion.setter_tid,0x1);
 }
 
-void clearWaypointReached()
+
+void clearMotionCmd()
 {
-    waypoint_reached = false;
-    d_reached = false;
-    a_reached = false;
+    current_motion.motion_done = false;
+    osSignalClear(current_motion.setter_tid, 0x1);
+
+    switch (current_motion.motion_type)
+    {
+        case motion_waypoint:
+            clearWaypoint();
+        break;
+        
+        default:
+        break;   
+    }
 }
 
-bool checkWaypointStatus()
+
+void clearWaypoint()
 {
-    return waypoint_reached;
+    wp_d_reached = false;
+    wp_a_reached = false;
 }
 
 } //namespace
\ No newline at end of file

--- a/Processes/Motion/motion.h	Sat Apr 13 22:41:00 2013 +0000
+++ b/Processes/Motion/motion.h	Sun Apr 14 10:49:51 2013 +0000
@@ -3,22 +3,22 @@
 
 #include "globals.h"
 #include "rtos.h"
-#include "math.h"
-#include "Kalman.h"
-#include "MotorControl.h"
-#include "supportfuncs.h"
 
 namespace motion
 {
 
 void motionlayer(void const *dummy);
+void waypoint_motion_handler();
 
-// can encapsulate mutex fully in motion with something like: bool set_new_wp_if_current_reached()
-void setNewWaypoint(Waypoint *new_wp);
-void setWaypointReached();
-void clearWaypointReached();
-bool checkWaypointStatus();
+// TODO: privatise (via unnamed namespace) non-API parts
+void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp);
+
+
 
+bool checkMotionStatus();
+
+
+// TODO: consider making mutex private and pushing all usage into API funcs
 extern Mutex waypoint_flag_mutex;
 
 }

--- a/globals.h	Sat Apr 13 22:41:00 2013 +0000
+++ b/globals.h	Sun Apr 14 10:49:51 2013 +0000
@@ -55,8 +55,8 @@
 
 */
 
-const PinName P_SERVO_LOWER_ARM = p5;
-const PinName P_SERVO_UPPER_ARM = p6;
+const PinName P_SERVO_LOWER_ARM = p25;
+const PinName P_SERVO_UPPER_ARM = p26;
 
 const PinName P_SERIAL_RX       = p14;
 const PinName P_DISTANCE_SENSOR = p15;
@@ -70,13 +70,13 @@
 const PinName P_MOT_RIGHT_A      = p24;
 const PinName P_MOT_RIGHT_B      = p23;
 
-const PinName P_ENC_RIGHT_A     = p26;
-const PinName P_ENC_RIGHT_B     = p25;
-const PinName P_ENC_LEFT_A      = p27;
-const PinName P_ENC_LEFT_B      = p28;
+const PinName P_ENC_RIGHT_A     = p28;//p26;
+const PinName P_ENC_RIGHT_B     = p27;//p25;
+const PinName P_ENC_LEFT_A      = p29;//p27;
+const PinName P_ENC_LEFT_B      = p30;//p28;
 
-const PinName P_COLOR_SENSOR_RED_UPPER = p29;
-const PinName P_COLOR_SENSOR_BLUE_UPPER = p30;
+const PinName P_COLOR_SENSOR_RED_UPPER = p13;//p29;
+const PinName P_COLOR_SENSOR_BLUE_UPPER = p12;//p30;
 const PinName P_COLOR_SENSOR_RED_LOWER = p11;
 const PinName P_COLOR_SENSOR_BLUE_LOWER = p10;