Diff: Processes/Motion/motion.cpp

Revision:

57:d434ceab6892

Parent:

54:99d3158c9207

Child:

64:c979fb1cd3b5

Child:

65:4709ff6c753c

--- a/Processes/Motion/motion.cpp	Sat Apr 13 19:01:45 2013 +0000
+++ b/Processes/Motion/motion.cpp	Sat Apr 13 21:29:35 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