A class for calculating steering angle calculations based on current and desired heading and specified intercept distance along the new path.
Diff: Steering.cpp
- Revision:
- 0:a3b128cdb78b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Steering.cpp Wed Apr 27 18:55:38 2011 +0000 @@ -0,0 +1,42 @@ +#include "Steering.h" +#include "math.h" + +/** create a new steering calculator for a particular vehicle + * + */ +Steering::Steering(float wheelbase, float track) + : _wheelbase(wheelbase) + , _track(track) + , _intercept(2.0) +{ +} + +void Steering::setIntercept(float intercept) +{ + _intercept = intercept; +} + +/** Calculate a steering angle based on relative bearing + * + */ +float Steering::calcSA(float theta) { + float radius; + float SA; + bool neg = (theta < 0); + + // I haven't had time to work out why the equation is slightly offset such + // that negative angle produces slightly less steering angle + // + if (neg) theta *= -1.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 (theta > 90.0) theta = 90.0; + + radius = _intercept/(2*sin(angle_radians(theta))); + SA = angle_degrees(asin(_wheelbase / (radius - _track/2))); + + if (neg) SA *= -1.0; + + return SA; +}