2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Motion/motion.cpp
- Committer:
- rsavitski
- Date:
- 2013-04-14
- Revision:
- 65:4709ff6c753c
- Parent:
- 57:d434ceab6892
- Child:
- 67:be3ea5450cc7
File content as of revision 65:4709ff6c753c:
//////////////////////////////////////////////////////////////////////////////// // Motion control unit //////////////////////////////////////////////////////////////////////////////// // Takes current state of the robot and target waypoint, // calculates desired forward and angular velocities and requests those from the motor control layer. //////////////////////////////////////////////////////////////////////////////// #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; // local to waypoint motion handler bool wp_d_reached = false; bool wp_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_motion.wp_ptr; // get current state from Kalman State current_state = Kalman::getState(); float delta_x = target_waypoint.x - current_state.x; float delta_y = target_waypoint.y - current_state.y; //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y); float distance_err = hypot(delta_x, delta_y); float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); // reversing if close to target and more optimal than forward movement bool reversing = false; 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); distance_err = -distance_err; } 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(); //TODO: consider refactoring if (abs(distance_err) < ((wp_d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) { wp_d_reached = true; angle_err = constrainAngle(target_waypoint.theta - current_state.theta); if (abs(angle_err) < target_waypoint.angle_threshold) { wp_a_reached = true; } } // 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 (wp_d_reached && wp_a_reached) { setWaypointReached(); } waypoint_flag_mutex.unlock(); // angular velocity controller const float p_gain_av = 0.5; //TODO: tune const float max_av = 0.5*PI; // radians per sec //TODO: tune // angle error [-pi, pi] float angular_v = p_gain_av * angle_err; // constrain range if (angular_v > max_av) angular_v = max_av; else if (angular_v < -max_av) angular_v = -max_av; // forward velocity controller const float p_gain_fv = 0.25; //TODO: tune float max_fv = 0.2; // meters per sec //TODO: tune float max_fv_reverse = 0.05; //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; // if reversing, robot is less stable hence a different cap is used if (reversing) 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); // constrain range if (forward_v > max_fv) forward_v = max_fv; else if (forward_v < -max_fv) forward_v = -max_fv; //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); // pass values to the motor control MotorControl::set_fwdcmd(forward_v); MotorControl::set_omegacmd(angular_v); } bool checkMotionStatus() { return current_motion.motion_done; } void setNewWaypoint(osThreadId setter_tid_in, 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() { current_motion.motion_done = true; osSignalSet(current_motion.setter_tid,0x1); } void clearMotionCmd() { current_motion.motion_done = false; osSignalClear(current_motion.setter_tid, 0x1); switch (current_motion.motion_type) { case motion_waypoint: clearWaypoint(); break; default: break; } } void clearWaypoint() { wp_d_reached = false; wp_a_reached = false; } } //namespace