Diff: Processes/Motion/motion.cpp

Revision:

34:a49197572737

Parent:

33:e3f633620816

Child:

35:e1678450feec

Child:

36:f8e7f0a72a3d

--- a/Processes/Motion/motion.cpp	Wed Apr 10 18:25:16 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 19:52:19 2013 +0000
@@ -16,7 +16,6 @@
     Waypoint target_waypoint = *AI::current_waypoint;
     
     // get current state from Kalman
-    using Kalman::State;
     State current_state = Kalman::getState();
     
     float delta_x = target_waypoint.x - current_state.x;
@@ -32,11 +31,14 @@
     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)
+        {
+            angle_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)
@@ -47,9 +49,9 @@
     AI::waypoint_flag_mutex.unlock();
     
     // angular velocity controller
-    const float p_gain_av = 0.3; //TODO: tune
+    const float p_gain_av = 0.8; //TODO: tune
     
-    const float max_av = 0.2*PI; // radians per sec //TODO: tune
+    const float max_av = 0.5*PI; // radians per sec //TODO: tune
     
     // angle error [-pi, pi]
     float angular_v = p_gain_av * angle_err;
@@ -62,7 +64,7 @@
     
     
     // forward velocity controller
-    const float p_gain_fv = 0.3; //TODO: tune
+    const float p_gain_fv = 0.8; //TODO: tune
     
     float max_fv = 0.2; // meters per sec //TODO: tune
     const float angle_envelope_exponent = 8.0; //TODO: tune