2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Motion/motion.cpp
- Revision:
- 35:f8e7f0a72a3d
- Parent:
- 33:a49197572737
- Child:
- 36:34f4b38039bb
--- a/Processes/Motion/motion.cpp Wed Apr 10 19:52:19 2013 +0000 +++ b/Processes/Motion/motion.cpp Wed Apr 10 20:07:33 2013 +0000 @@ -6,7 +6,7 @@ //////////////////////////////////////////////////////////////////////////////// #include "motion.h" - +DigitalOut OLED4(LED4); namespace motion { @@ -31,18 +31,17 @@ 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) + if (abs(constrainAngle(target_waypoint.theta - current_state.theta)) < 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) { + OLED4 = 1; AI::setWaypointReached(); return; }