Steering.cpp

00001 #include "Steering.h"
00002 #include "math.h"
00003 
00004 /** create a new steering calculator for a particular vehicle
00005  *
00006  */
00007 Steering::Steering(float wheelbase, float track)
00008     : _wheelbase(wheelbase)
00009     , _track(track)
00010     , _intercept(2.0)
00011 {
00012 }
00013 
00014 void Steering::setIntercept(float intercept)
00015 {
00016     _intercept = intercept;
00017 }
00018 
00019 /** Calculate a steering angle based on relative bearing
00020  *
00021  */
00022 float Steering::calcSA(float theta) {
00023     float radius;
00024     float SA;
00025     bool neg = (theta < 0);
00026 
00027     // I haven't had time to work out why the equation is slightly offset such
00028     // that negative angle produces slightly less steering angle
00029     //
00030     if (neg) theta *= -1.0;    
00031     
00032     // The equation peaks out at 90* so clamp theta artifically to 90, so that
00033     // if theta is actually > 90, we select max steering
00034     if (theta > 90.0) theta = 90.0;
00035     
00036     radius = _intercept/(2*sin(angle_radians(theta)));
00037     SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
00038 
00039     if (neg) SA *= -1.0;
00040 
00041     return SA;
00042 }