A class for calculating steering angle calculations based on current and desired heading and specified intercept distance along the new path.
Steering.cpp@0:a3b128cdb78b, 2011-04-27 (annotated)
- Committer:
- shimniok
- Date:
- Wed Apr 27 18:55:38 2011 +0000
- Revision:
- 0:a3b128cdb78b
Initial release
Who changed what in which revision?
User | Revision | Line number | New 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 | } |