This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Processes/Motion/motion.cpp
- Revision:
- 52:bffe5f7c39a3
- Parent:
- 40:fefdbb9b5968
- Child:
- 54:99d3158c9207
--- a/Processes/Motion/motion.cpp Thu Apr 11 18:49:11 2013 +0000 +++ b/Processes/Motion/motion.cpp Fri Apr 12 17:29:38 2013 +0000 @@ -35,14 +35,25 @@ float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); + // reversing + bool reversing = false; + if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) + { + 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(); - if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) + if (abs(distance_err) < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) { d_reached = true; - distance_err = 0; + //distance_err = 0; - angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta); + angle_err = constrainAngle(target_waypoint.theta - current_state.theta); if (abs(angle_err) < target_waypoint.angle_threshold) { @@ -78,13 +89,18 @@ 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 + float max_fv_reverse = 0.05; //TODO: tune + const float angle_envelope_exponent = 32.0; //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/2), angle_envelope_exponent); + max_fv = max_fv * pow(cos(angle_err_saved/2), angle_envelope_exponent); // constrain range if (forward_v > max_fv)