Diff: Actuators/Steering/Steering.cpp

Revision:

1:cb84b477886c

Parent:

0:a6a169de725f

Child:

2:fbc6e3cf3ed8

--- a/Actuators/Steering/Steering.cpp	Mon May 27 13:26:03 2013 +0000
+++ b/Actuators/Steering/Steering.cpp	Tue May 28 13:58:35 2013 +0000
@@ -1,6 +1,8 @@
 #include "Steering.h"
 #include "math.h"
 
+#include "mbed.h"
+
 /** create a new steering calculator for a particular vehicle
  *
  */
@@ -71,6 +73,7 @@
  */
 float Steering::crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy)
 {
+/*
     // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
     float Rx = (Bx - Ax);
     // compute run for prev wpt to bot; or compute vector offset by A(x,y)
@@ -93,8 +96,9 @@
     float NBx = Nx-Bx;
     float NBy = Ny-By;
     float cte = sqrtf(NBx*NBx + NBy*NBy);
-
     return cte;
+*/
+    return 0;
 }
 
 
@@ -106,7 +110,7 @@
     float Ly = Cy - Ay;
     // Robot vector
     float Rx = Bx - Ax;
-    float Ry = Cy - Ay;
+    float Ry = By - Ay;
 
     // Find the goal point, a projection of the bot vector onto the current leg, moved ahead
     // along the path by the lookahead distance
@@ -116,21 +120,19 @@
     float LAy = (proj+_intercept)*Ly/legLength + Ay;
     // 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.
+    // angle to trace that circle. (x,y because 0 deg points up not right)
     float brg = 180*atan2(LAx-Rx,LAy-Ry)/PI;
     // 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;
+    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 (relBrg > 90.0) relBrg = 90.0;
+    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));
     // optionally, limit radius min/max
@@ -138,6 +140,8 @@
     // 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);
+        
     return SA;
 }