A class for calculating steering angle calculations based on current and desired heading and specified intercept distance along the new path.

Dependents:   AVC_20110423

Committer:
shimniok
Date:
Wed Apr 27 18:55:38 2011 +0000
Revision:
0:a3b128cdb78b
Initial release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a3b128cdb78b 1 #include "Steering.h"
shimniok 0:a3b128cdb78b 2 #include "math.h"
shimniok 0:a3b128cdb78b 3
shimniok 0:a3b128cdb78b 4 /** create a new steering calculator for a particular vehicle
shimniok 0:a3b128cdb78b 5 *
shimniok 0:a3b128cdb78b 6 */
shimniok 0:a3b128cdb78b 7 Steering::Steering(float wheelbase, float track)
shimniok 0:a3b128cdb78b 8 : _wheelbase(wheelbase)
shimniok 0:a3b128cdb78b 9 , _track(track)
shimniok 0:a3b128cdb78b 10 , _intercept(2.0)
shimniok 0:a3b128cdb78b 11 {
shimniok 0:a3b128cdb78b 12 }
shimniok 0:a3b128cdb78b 13
shimniok 0:a3b128cdb78b 14 void Steering::setIntercept(float intercept)
shimniok 0:a3b128cdb78b 15 {
shimniok 0:a3b128cdb78b 16 _intercept = intercept;
shimniok 0:a3b128cdb78b 17 }
shimniok 0:a3b128cdb78b 18
shimniok 0:a3b128cdb78b 19 /** Calculate a steering angle based on relative bearing
shimniok 0:a3b128cdb78b 20 *
shimniok 0:a3b128cdb78b 21 */
shimniok 0:a3b128cdb78b 22 float Steering::calcSA(float theta) {
shimniok 0:a3b128cdb78b 23 float radius;
shimniok 0:a3b128cdb78b 24 float SA;
shimniok 0:a3b128cdb78b 25 bool neg = (theta < 0);
shimniok 0:a3b128cdb78b 26
shimniok 0:a3b128cdb78b 27 // I haven't had time to work out why the equation is slightly offset such
shimniok 0:a3b128cdb78b 28 // that negative angle produces slightly less steering angle
shimniok 0:a3b128cdb78b 29 //
shimniok 0:a3b128cdb78b 30 if (neg) theta *= -1.0;
shimniok 0:a3b128cdb78b 31
shimniok 0:a3b128cdb78b 32 // The equation peaks out at 90* so clamp theta artifically to 90, so that
shimniok 0:a3b128cdb78b 33 // if theta is actually > 90, we select max steering
shimniok 0:a3b128cdb78b 34 if (theta > 90.0) theta = 90.0;
shimniok 0:a3b128cdb78b 35
shimniok 0:a3b128cdb78b 36 radius = _intercept/(2*sin(angle_radians(theta)));
shimniok 0:a3b128cdb78b 37 SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 0:a3b128cdb78b 38
shimniok 0:a3b128cdb78b 39 if (neg) SA *= -1.0;
shimniok 0:a3b128cdb78b 40
shimniok 0:a3b128cdb78b 41 return SA;
shimniok 0:a3b128cdb78b 42 }