Diff: Processes/Motion/motion.cpp

Revision:

39:c9058a401410

Parent:

38:6ecf0d21e492

Child:

40:44d3dea4adcc

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