Diff: Processes/Motion/motion.cpp

Revision:

32:e3f633620816

Parent:

26:7cb3a21d9a2e

Parent:

30:791739422122

Child:

33:a49197572737

--- a/Processes/Motion/motion.cpp	Wed Apr 10 18:04:47 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 18:25:16 2013 +0000
@@ -6,17 +6,12 @@
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
-#include "supportfuncs.h"
-#include "Kalman.h"
 
 namespace motion
 {
 
 void motionlayer(void const *dummy)
-{
-    //TODO: current_waypoint global in AI layer
-    //TODO: threshold for deciding that the waypoint has been achieved
-    
+{    
     // get target waypoint from AI
     Waypoint target_waypoint = *AI::current_waypoint;
     
@@ -33,6 +28,23 @@
     
     float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
     
+    // is the waypoint reached
+    if (distance_err < target_waypoint.pos_threshold)
+    {
+        distance_err = 0;
+    }
+    if (abs(angle_err) < 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)
+    {
+        AI::setWaypointReached();
+        return;
+    }
+    AI::waypoint_flag_mutex.unlock();
     
     // angular velocity controller
     const float p_gain_av = 0.3; //TODO: tune
@@ -69,7 +81,7 @@
         
     //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
     
-    //TODO: put into motor control
+    // pass values to the motor control
     MotorControl::set_fwdcmd(forward_v);
     MotorControl::set_omegacmd(angular_v);
 }