Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Diff: Actuators/Steering/Steering.cpp
- Revision:
- 1:cb84b477886c
- Parent:
- 0:a6a169de725f
- Child:
- 2:fbc6e3cf3ed8
diff -r a6a169de725f -r cb84b477886c Actuators/Steering/Steering.cpp --- 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; }