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:
- 3:42f3821c4e54
- Parent:
- 2:fbc6e3cf3ed8
- Child:
- 15:01fb4916a5cd
diff -r fbc6e3cf3ed8 -r 42f3821c4e54 Actuators/Steering/Steering.cpp --- 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))) ); }