A class for calculating steering angle calculations based on current and desired heading and specified intercept distance along the new path.
Steering.cpp
- Committer:
- shimniok
- Date:
- 2011-04-27
- Revision:
- 0:a3b128cdb78b
File content as of revision 0:a3b128cdb78b:
#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; }