2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Motion/motion.cpp
- Revision:
- 33:a49197572737
- Parent:
- 32:e3f633620816
- Child:
- 34:e1678450feec
- Child:
- 35:f8e7f0a72a3d
diff -r e3f633620816 -r a49197572737 Processes/Motion/motion.cpp --- a/Processes/Motion/motion.cpp Wed Apr 10 18:25:16 2013 +0000 +++ b/Processes/Motion/motion.cpp Wed Apr 10 19:52:19 2013 +0000 @@ -16,7 +16,6 @@ Waypoint target_waypoint = *AI::current_waypoint; // get current state from Kalman - using Kalman::State; State current_state = Kalman::getState(); float delta_x = target_waypoint.x - current_state.x; @@ -32,11 +31,14 @@ if (distance_err < target_waypoint.pos_threshold) { distance_err = 0; + angle_err = constrainAngle(target_waypoint.theta - current_state.theta); + + if (abs(angle_err) < target_waypoint.angle_threshold) + { + angle_err = 0; + } } - if (abs(angle_err) < target_waypoint.angle_threshold) - { - angle_err = 0; - } + AI::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 (distance_err == 0 && angle_err == 0) @@ -47,9 +49,9 @@ AI::waypoint_flag_mutex.unlock(); // angular velocity controller - const float p_gain_av = 0.3; //TODO: tune + const float p_gain_av = 0.8; //TODO: tune - const float max_av = 0.2*PI; // radians per sec //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; @@ -62,7 +64,7 @@ // forward velocity controller - const float p_gain_fv = 0.3; //TODO: tune + const float p_gain_fv = 0.8; //TODO: tune float max_fv = 0.2; // meters per sec //TODO: tune const float angle_envelope_exponent = 8.0; //TODO: tune