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;
     }