Diff: Actuators/Steering/Steering.cpp

Revision:

0:826c6171fc1b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/Steering/Steering.cpp	Wed Jun 20 14:57:48 2012 +0000
@@ -0,0 +1,157 @@
+#include "Steering.h"
+#include "math.h"
+
+/** create a new steering calculator for a particular vehicle
+ *
+ */
+Steering::Steering(float wheelbase, float track)
+    : _wheelbase(wheelbase)
+    , _track(track)
+    , _intercept(2.0)
+{
+}
+
+void Steering::setIntercept(float intercept)
+{
+    _intercept = intercept;
+}
+
+/** Calculate a steering angle based on relative bearing
+ *
+ */
+float Steering::calcSA(float theta) {
+    return calcSA(theta, -1.0); // call with no limit
+}
+
+/** calcSA
+ * minRadius -- radius limit (minRadius < 0 disables limiting)
+ */
+float Steering::calcSA(float theta, float minRadius) {
+    float radius;
+    float SA;
+    bool neg = (theta < 0);
+
+    // I haven't had time to work out why the equation is slightly offset such
+    // that negative angle produces slightly less steering angle
+    //
+    if (neg) theta = -theta;
+    
+    // The equation peaks out at 90* so clamp theta artifically to 90, so that
+    // if theta is actually > 90, we select max steering
+    if (theta > 90.0) theta = 90.0;
+
+    // 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)) );
+    
+    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)));
+    // 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.
+    // To consider this, we'd need to measure the understeer gradient of
+    // the vehicle (thanks to Project240 for this insight) and include
+    // that in the calculation.
+
+    if (neg) SA = -SA;
+
+    return SA;
+}
+
+/**
+ * Bxy - robot coordinates
+ * Axy - previous waypoint coords
+ * Cxy - next waypoint coords
+ */
+float Steering::crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy)
+{
+  // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
+  float Rx = (Bx - Ax);
+  // compute run for prev wpt to bot; or compute vector offset by A(x,y)
+  float Ry = (By - Ay);
+  // dx is the run for the path
+  float dx = Cx - Ax;
+  // dy is the rise for the path
+  float dy = Cy - Ay;
+  // this is hypoteneuse length squared
+  float ACd2 = dx*dx+dy*dy;
+  // length of hyptoenuse
+  float ACd = sqrtf( ACd2 );
+  
+  float Rd = Rx*dx + Ry*dy;  
+  float t = Rd / ACd2;
+  // nearest point on current segment
+  float Nx = Ax + dx*t; 
+  float Ny = Ay + dy*t;  
+  // Cross track error
+  float NBx = Nx-Bx;
+  float NBy = Ny-By;
+  float cte = sqrtf(NBx*NBx + NBy*NBy);
+  
+  return cte;
+}
+
+
+
+float Steering::purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
+{
+  float SA;
+
+  // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
+  float Rx = (Bx - Ax);
+  // compute run for prev wpt to bot; or compute vector offset by A(x,y)
+  float Ry = (By - Ay);
+  // dx is the run for the path
+  float dx = Cx - Ax;
+  // dy is the rise for the path
+  float dy = Cy - Ay;
+  // this is hypoteneuse length squared
+  float ACd2 = dx*dx+dy*dy;
+  // length of hyptoenuse
+  float ACd = sqrtf( ACd2 );
+  
+  float Rd = Rx*dx + Ry*dy;  
+  float t = Rd / ACd2;
+  // nearest point on current segment
+  float Nx = Ax + dx*t; 
+  float Ny = Ay + dy*t;  
+  // Cross track error
+  float NBx = Nx-Bx;
+  float NBy = Ny-By;
+  float cte = sqrtf(NBx*NBx + NBy*NBy);
+    float NGd;
+
+  float myLookAhead;
+  
+  if (cte <= _intercept) {
+    myLookAhead = _intercept;
+  } else {
+    myLookAhead = _intercept + cte;
+  }
+  
+  NGd = sqrt( myLookAhead*myLookAhead - cte*cte );
+  float Gx = NGd * dx/ACd + Nx;
+  float Gy = NGd * dy/ACd + Ny;
+  
+  float hdgr = hdg*PI/180;
+  
+  float BGx = (Gx-Bx)*cos(hdgr) - (Gy-By)*sin(hdgr);
+  float c = (2 * BGx) / (myLookAhead*myLookAhead);
+
+  float radius;
+  
+  if (c != 0) {
+    radius = 1/c;
+  } else {
+    radius = 999999.0;
+  }
+
+  // Now calculate steering angle based on wheelbase and track width
+  SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
+
+  return SA;
+}
\ No newline at end of file