Diff: Processes/Motion/motion.cpp

Revision:

25:b16f1045108f

Parent:

24:50805ef8c499

Child:

26:7cb3a21d9a2e

Child:

30:791739422122

--- a/Processes/Motion/motion.cpp	Tue Apr 09 20:37:59 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -6,6 +6,7 @@
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
+#include "supportfuncs.h"
 
 namespace motion
 {
@@ -24,15 +25,17 @@
     float delta_x = target_waypoint.x - current_state.x;
     float delta_y = target_waypoint.y - current_state.y;
     
-    float distance_err = sqrt(delta_x*delta_x + delta_y*delta_y);
+    //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y);
     
-    float angle_err = atan2(delta_y, delta_x);
+    float distance_err = hypot(delta_x, delta_y);
+    
+    float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
     
     
     // angular velocity controller
-    const float p_gain_av = 1.0; //TODO: tune
+    const float p_gain_av = 0.3; //TODO: tune
     
-    const float max_av = PI; // radians per sec //TODO: tune
+    const float max_av = 0.2*PI; // radians per sec //TODO: tune
     
     // angle error [-pi, pi]
     float angular_v = p_gain_av * angle_err;
@@ -45,9 +48,9 @@
     
     
     // forward velocity controller
-    const float p_gain_fv = 1.0; //TODO: tune
+    const float p_gain_fv = 0.3; //TODO: tune
     
-    float max_fv = 1.0; // meters per sec //TODO: tune
+    float max_fv = 0.2; // meters per sec //TODO: tune
     const float angle_envelope_exponent = 8.0; //TODO: tune
     
     // control, distance_err in meters
@@ -61,10 +64,12 @@
         forward_v = max_fv;
     else if (forward_v < -max_fv)
         forward_v = -max_fv;
+        
+    //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
     
     //TODO: put into motor control
     MotorControl::set_fwdcmd(forward_v);
-    MotorControl::set_thetacmd(angular_v);
+    MotorControl::set_omegacmd(angular_v);
 }
 
 } //namespace
\ No newline at end of file