Diff: Actuators/Steering/Steering.cpp

Revision:

15:01fb4916a5cd

Parent:

3:42f3821c4e54

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