Diff: Actuators/Steering/Steering.cpp

Revision:

2:fbc6e3cf3ed8

Parent:

1:cb84b477886c

Child:

3:42f3821c4e54

diff -r cb84b477886c -r fbc6e3cf3ed8 Actuators/Steering/Steering.cpp
--- a/Actuators/Steering/Steering.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/Actuators/Steering/Steering.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -6,10 +6,10 @@
 /** create a new steering calculator for a particular vehicle
  *
  */
-Steering::Steering(float wheelbase, float track)
-    : _wheelbase(wheelbase)
-    , _track(track)
-    , _intercept(2.0)
+Steering::Steering(float wheelbase, float track):
+    _wheelbase(wheelbase),
+    _track(track),
+    _intercept(2.0)
 {
 }
 
@@ -101,6 +101,7 @@
     return 0;
 }
 
+//static int skip=0;
 
 float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
 {
@@ -116,31 +117,37 @@
     // along the path by the lookahead distance
     float legLength = sqrtf(Lx*Lx + Ly*Ly); // ||L||
     float proj = (Lx*Rx + Ly*Ry)/legLength; // R dot L/||L||, projection magnitude, bot vector onto leg vector
-    float LAx = (proj+_intercept)*Lx/legLength + Ax; // find projection point + lookahead, along leg
-    float LAy = (proj+_intercept)*Ly/legLength + Ay;
+    float LAx = (proj + _intercept)*Lx/legLength; // find projection point + lookahead, along leg, relative to Bx
+    float LAy = (proj + _intercept)*Ly/legLength;
     // 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 = 180*atan2(LAx-Rx,LAy-Ry)/PI;
+    float brg = 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;
+    float relBrg = brg - hdg;
     if (relBrg < -180.0) relBrg += 360.0;
     if (relBrg >= 180.0) relBrg -= 360.0;
-    // I haven't had time to work out why the equation is asymmetrical, that is, 
-    // negative angle produces slightly less steering angle. So instead, just use
-    // absolute value and restore sign later.
-    float sign = (relBrg < 0) ? -1 : 1;
     // 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;
     // Compute radius based on intercept distance and specified angle
-    float radius = _intercept/(2*sin(fabs(relBrg)*PI/180));
+    float radius = _intercept/(2*sin(angle_radians(fabs(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 = sign * angle_degrees(asin(_wheelbase / (radius - _track/2)));
-
-    //fprintf(stdout, "R(%.4f,%.4f) A(%.4f,%.4f) C(%.4f,%.4f) L(%.4f,%.4f) b=%.2f rb=%.2f sa=%.2f\n", Bx, By, Ax, Ay, Cx, Cy, Lx, Ly, brg, relBrg, SA);
+    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;
 }