2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
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