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()