Diff: Actuators/Steering/Steering.cpp

Revision:

3:42f3821c4e54

Parent:

2:fbc6e3cf3ed8

Child:

15:01fb4916a5cd

--- a/Actuators/Steering/Steering.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/Actuators/Steering/Steering.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -1,7 +1,7 @@
+#include "mbed.h"
 #include "Steering.h"
 #include "math.h"
-
-#include "mbed.h"
+#include "util.h"
 
 /** create a new steering calculator for a particular vehicle
  *
@@ -105,13 +105,13 @@
 
 float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
 {
-    float SA;
     // Leg vector
     float Lx = Cx - Ax;
     float Ly = Cy - Ay;
     // Robot vector
     float Rx = Bx - Ax;
     float Ry = By - Ay;
+    float sign = 1;
 
     // Find the goal point, a projection of the bot vector onto the current leg, moved ahead
     // along the path by the lookahead distance
@@ -122,34 +122,27 @@
     // Compute a circle that is tangential to bot heading and intercepts bot
     // and goal point (LAx,LAy), the intercept circle. Then compute the steering
     // angle to trace that circle. (x,y because 0 deg points up not right)
-    float brg = angle_degrees(atan2(LAx-Rx,LAy-Ry));
-    if (brg >= 360.0) brg -= 360.0;
-    if (brg < 0) brg += 360.0;
+    float brg = clamp360( angle_degrees(atan2(LAx-Rx,LAy-Ry)) );
+    //if (brg >= 360.0) brg -= 360.0;
+    //if (brg < 0) brg += 360.0;
     // would be nice to add in some noise to heading info
-    float relBrg = brg - hdg;
-    if (relBrg < -180.0) relBrg += 360.0;
-    if (relBrg >= 180.0) relBrg -= 360.0;
-    // The equation peaks out at 90* so clamp theta artifically to 90, so that
-    // if theta is actually > 90, we select max steering
-    if (fabs(relBrg) > 90.0) relBrg = 90.0;
+    float relBrg = clamp180(brg - hdg);
+    // The steering angle equation actually peaks at relBrg == 90 so just clamp to this
+    if (relBrg > 89.5) {
+        relBrg = 89.5;
+    } else if (relBrg < -89.5) {
+        relBrg = -89.5;
+    }
     // Compute radius based on intercept distance and specified angle
-    float radius = _intercept/(2*sin(angle_radians(fabs(relBrg))));
+    // The sin equation will produce a negative radius, which causes problems
+    // when subtracting track/2.0, so just take absolute value and multiply sign
+    // later on
+    sign = (relBrg < 0) ? -1 : 1;
+    float radius = _intercept/fabs(2*sin(angle_radians(relBrg)));
     // optionally, limit radius min/max
     // Now compute the steering angle to achieve the circle of 
     // Steering angle is based on wheelbase and track width
-    SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
-    // I haven't had time to work out why the equation is asymmetrical, that is, 
-    // negative angle produces slightly less steering angle.
-    if (relBrg < 0) SA = -SA; 
-    
-    /*
-    if (++skip >= 20) {
-        fprintf(stdout, "R(%.4f,%.4f) A(%.4f,%.4f) C(%.4f,%.4f) LA(%.4f,%.4f) h=%.2f b=%.2f rb=%.2f sa=%.2f\n", Bx, By, Ax, Ay, Cx, Cy, LAx, LAy, hdg, brg, relBrg, SA);
-        skip = 0;
-    }
-    */
-        
-    return SA;
+    return ( sign * angle_degrees(asin(_wheelbase / (radius - _track/2.0))) );
 }