2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Motion/motion.cpp
- Committer:
- rsavitski
- Date:
- 2013-04-11
- Revision:
- 40:fefdbb9b5968
- Parent:
- 39:44d3dea4adcc
- Child:
- 52:bffe5f7c39a3
File content as of revision 40:fefdbb9b5968:
//////////////////////////////////////////////////////////////////////////////// // 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" namespace motion { Mutex waypoint_flag_mutex; Waypoint *current_waypoint = NULL; bool waypoint_reached = false; // is current waypoint reached bool d_reached = false; bool a_reached = false; void motionlayer(void const *dummy) { // save target waypoint Waypoint target_waypoint = *current_waypoint; // 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); // is the waypoint reached waypoint_flag_mutex.lock(); if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) { d_reached = true; distance_err = 0; angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta); if (abs(angle_err) < target_waypoint.angle_threshold) { a_reached = true; //angle_err = 0; } } 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) { 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.5; //TODO: tune float max_fv = 0.2; // meters per sec //TODO: tune const float angle_envelope_exponent = 8.0; //TODO: tune // control, distance_err in meters float forward_v = p_gain_fv * distance_err; // control the forward velocity envelope based on angular error max_fv = max_fv * pow(cos(angle_err/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); } void setNewWaypoint(Waypoint *new_wp) { current_waypoint = new_wp; } void setWaypointReached() { waypoint_reached = true; } void clearWaypointReached() { waypoint_reached = false; d_reached = false; a_reached = false; } bool checkWaypointStatus() { return waypoint_reached; } } //namespace