![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Diff: Actuators/Steering/Steering.cpp
- Revision:
- 15:01fb4916a5cd
- Parent:
- 3:42f3821c4e54
diff -r 86ee4d840f6b -r 01fb4916a5cd Actuators/Steering/Steering.cpp --- a/Actuators/Steering/Steering.cpp Thu Nov 29 16:59:39 2018 +0000 +++ b/Actuators/Steering/Steering.cpp Thu Nov 29 17:11:34 2018 +0000 @@ -1,23 +1,65 @@ #include "mbed.h" +#include "globals.h" #include "Steering.h" #include "math.h" #include "util.h" -/** create a new steering calculator for a particular vehicle - * - */ -Steering::Steering(float wheelbase, float track): - _wheelbase(wheelbase), - _track(track), - _intercept(2.0) +Steering::Steering(PinName pin): + _steering(pin), + _scale(0), + _track(0), + _wheelbase(0), + _intercept(0) { } -void Steering::setIntercept(float intercept) + +void Steering::initSteering() { + _steering = 0.4; + // TODO: 3 parameterize this in config file ? + _steering.calibrate(0.005, 45.0); +} + +void Steering::setScale(float scale) { + _scale = scale; +} + +void Steering::setIntercept(float intercept) { _intercept = intercept; } +void Steering::setWheelbase(float wheelbase) { + _wheelbase = wheelbase; +} + +void Steering::setTrack(float track) { + _track = track; +} + +// TODO 3 convert steering to Servo2 library +void Steering::setSteering(float steerAngle) +{ + // Convert steerAngle to servo value + // Testing determined near linear conversion between servo ms setting and steering angle + // up to 20*. Assumes a particular servo library with range = 0.005 + // In that case, f(SA) = servoPosition = 0.500 + SA/762.5 + // between 20 and 24* the slope is approximately 475 + // What if we ignore the linearity and just set to a max angle + // also range is 0.535-0.460 --> slope = 800 + // steering = 0.500 + (double) steerAngle / 762.5; + // + _steering = 0.500 + (double) steerAngle * _scale; +} + + +float Steering::operator =(float steerAngle) +{ + setSteering(steerAngle); + return steerAngle; +} + + /** Calculate a steering angle based on relative bearing * */ @@ -46,14 +88,14 @@ // Compute |radius| based on intercept distance and specified angle with extra gain to // overcome steering slop, misalignment, sidehills, etc. - radius = _intercept / ( 2 * sin(angle_radians(theta)) ); + radius = _intercept / ( 2 * sin(toRadians(theta)) ); if (minRadius > 0) { if (radius < minRadius) radius = minRadius; } // Now calculate steering angle based on wheelbase and track width - SA = angle_degrees(asin(_wheelbase / (radius - _track/2))); + SA = toDegrees(asin(_wheelbase / (radius - _track/2))); // The above ignores the effect of speed on required steering angle. // Even when under the limits of traction, understeer means more angle // is required to achieve a turn at higher speeds than lower speeds. @@ -122,7 +164,7 @@ // Compute a circle that is tangential to bot heading and intercepts bot // and goal point (LAx,LAy), the intercept circle. Then compute the steering // angle to trace that circle. (x,y because 0 deg points up not right) - float brg = clamp360( angle_degrees(atan2(LAx-Rx,LAy-Ry)) ); + float brg = clamp360( toDegrees(atan2(LAx-Rx,LAy-Ry)) ); //if (brg >= 360.0) brg -= 360.0; //if (brg < 0) brg += 360.0; // would be nice to add in some noise to heading info @@ -138,11 +180,11 @@ // when subtracting track/2.0, so just take absolute value and multiply sign // later on sign = (relBrg < 0) ? -1 : 1; - float radius = _intercept/fabs(2*sin(angle_radians(relBrg))); + float radius = _intercept/fabs(2*sin(toRadians(relBrg))); // optionally, limit radius min/max // Now compute the steering angle to achieve the circle of // Steering angle is based on wheelbase and track width - return ( sign * angle_degrees(asin(_wheelbase / (radius - _track/2.0))) ); + return ( sign * toDegrees(asin(_wheelbase / (radius - _track/2.0))) ); } @@ -200,7 +242,7 @@ } // Now calculate steering angle based on wheelbase and track width - SA = angle_degrees(asin(_wheelbase / (radius - _track/2))); + SA = toDegrees(asin(_wheelbase / (radius - _track/2))); return SA; -} \ No newline at end of file +}