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;
}