2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Revision 57:d434ceab6892, committed 2013-04-13
- Comitter:
- rsavitski
- Date:
- Sat Apr 13 21:29:35 2013 +0000
- Parent:
- 56:ed585a82092b
- Child:
- 58:ff2121f34e3b
- Commit message:
- refactored motion with proxy movement object, working
Changed in this revision
--- a/Processes/AI/ai.cpp Sat Apr 13 19:01:45 2013 +0000 +++ b/Processes/AI/ai.cpp Sat Apr 13 21:29:35 2013 +0000 @@ -19,7 +19,7 @@ current_waypoint.pos_threshold = 0.01; current_waypoint.angle_threshold = 0.02*PI; - motion::setNewWaypoint(¤t_waypoint); + motion::setNewWaypoint(Thread::gettid(),¤t_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,7 +29,7 @@ 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); @@ -39,9 +39,8 @@ //arm offset current_waypoint.x += 0.0425*cos(current_waypoint.theta); current_waypoint.y += 0.0425*sin(current_waypoint.theta); - - motion::clearWaypointReached(); - motion::setNewWaypoint(¤t_waypoint); + + motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); } motion::waypoint_flag_mutex.unlock();
--- 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
--- a/Processes/Motion/motion.h Sat Apr 13 19:01:45 2013 +0000 +++ b/Processes/Motion/motion.h Sat Apr 13 21:29:35 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; }