Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
Diff: Processes/Motion/motion.cpp
- Revision:
- 39:c9058a401410
- Parent:
- 38:6ecf0d21e492
- Child:
- 40:44d3dea4adcc
diff -r 6ecf0d21e492 -r c9058a401410 Processes/Motion/motion.cpp --- a/Processes/Motion/motion.cpp Wed Apr 10 20:44:29 2013 +0000 +++ b/Processes/Motion/motion.cpp Wed Apr 10 22:30:09 2013 +0000 @@ -6,8 +6,7 @@ //////////////////////////////////////////////////////////////////////////////// #include "motion.h" -DigitalOut OLED4(LED4); -DigitalOut OLED1(LED1); + namespace motion { @@ -36,8 +35,10 @@ { d_reached = true; distance_err = 0; - OLED1 = 1; - if (abs(constrainAngle(target_waypoint.theta - current_state.theta)) < target_waypoint.angle_threshold) + + angle_err = 0.2*constrainAngle(target_waypoint.theta - current_state.theta); + + if (abs(angle_err) < target_waypoint.angle_threshold) { a_reached = true; angle_err = 0; @@ -47,9 +48,7 @@ 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 (d_reached && a_reached) { - OLED4 = 1; AI::setWaypointReached(); - return; } AI::waypoint_flag_mutex.unlock();