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:
- 2:fbc6e3cf3ed8
- Parent:
- 1:cb84b477886c
- Child:
- 3:42f3821c4e54
--- 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; }