2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Motion/motion.cpp
- Revision:
- 40:fefdbb9b5968
- Parent:
- 39:44d3dea4adcc
- Child:
- 52:bffe5f7c39a3
--- a/Processes/Motion/motion.cpp Thu Apr 11 17:23:07 2013 +0000 +++ b/Processes/Motion/motion.cpp Thu Apr 11 18:49:11 2013 +0000 @@ -15,7 +15,8 @@ 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) { @@ -34,12 +35,9 @@ float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); - bool d_reached = false; - bool a_reached = false; - // is the waypoint reached waypoint_flag_mutex.lock(); - if (distance_err < ((checkWaypointStatus()) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) + if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) { d_reached = true; distance_err = 0; @@ -114,6 +112,8 @@ void clearWaypointReached() { waypoint_reached = false; + d_reached = false; + a_reached = false; } bool checkWaypointStatus()