Steering.cpp

00001 #include "Steering.h"
00002 #include "math.h"
00003 
00004 /** create a new steering calculator for a particular vehicle
00005  *
00006  */
00007 Steering::Steering(float wheelbase, float track)
00008     : _wheelbase(wheelbase)
00009     , _track(track)
00010     , _intercept(2.0)
00011 {
00012 }
00013 
00014 void Steering::setIntercept(float intercept)
00015 {
00016     _intercept = intercept;
00017 }
00018 
00019 /** Calculate a steering angle based on relative bearing
00020  *
00021  */
00022 float Steering::calcSA(float theta) {
00023     return calcSA(theta, -1.0); // call with no limit
00024 }
00025 
00026 /** calcSA
00027  * minRadius -- radius limit (minRadius < 0 disables limiting)
00028  */
00029 float Steering::calcSA(float theta, float minRadius) {
00030     float radius;
00031     float SA;
00032     bool neg = (theta < 0);
00033 
00034     // I haven't had time to work out why the equation is slightly offset such
00035     // that negative angle produces slightly less steering angle
00036     //
00037     if (neg) theta = -theta;
00038     
00039     // The equation peaks out at 90* so clamp theta artifically to 90, so that
00040     // if theta is actually > 90, we select max steering
00041     if (theta > 90.0) theta = 90.0;
00042 
00043     // Compute |radius| based on intercept distance and specified angle with extra gain to 
00044     // overcome steering slop, misalignment, sidehills, etc.
00045     radius = _intercept / ( 2 * sin(angle_radians(theta)) );
00046     
00047     if (minRadius > 0) {
00048         if (radius < minRadius) radius = minRadius;
00049     }
00050 
00051     // Now calculate steering angle based on wheelbase and track width
00052     SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
00053     // The above ignores the effect of speed on required steering angle.
00054     // Even when under the limits of traction, understeer means more angle
00055     // is required to achieve a turn at higher speeds than lower speeds.
00056     // To consider this, we'd need to measure the understeer gradient of
00057     // the vehicle (thanks to Project240 for this insight) and include
00058     // that in the calculation.
00059 
00060     if (neg) SA = -SA;
00061 
00062     return SA;
00063 }
00064 
00065 /**
00066  * Bxy - robot coordinates
00067  * Axy - previous waypoint coords
00068  * Cxy - next waypoint coords
00069  */
00070 float Steering::crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy)
00071 {
00072   // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
00073   float Rx = (Bx - Ax);
00074   // compute run for prev wpt to bot; or compute vector offset by A(x,y)
00075   float Ry = (By - Ay);
00076   // dx is the run for the path
00077   float dx = Cx - Ax;
00078   // dy is the rise for the path
00079   float dy = Cy - Ay;
00080   // this is hypoteneuse length squared
00081   float ACd2 = dx*dx+dy*dy;
00082   // length of hyptoenuse
00083   float ACd = sqrtf( ACd2 );
00084   
00085   float Rd = Rx*dx + Ry*dy;  
00086   float t = Rd / ACd2;
00087   // nearest point on current segment
00088   float Nx = Ax + dx*t; 
00089   float Ny = Ay + dy*t;  
00090   // Cross track error
00091   float NBx = Nx-Bx;
00092   float NBy = Ny-By;
00093   float cte = sqrtf(NBx*NBx + NBy*NBy);
00094   
00095   return cte;
00096 }
00097 
00098 
00099 
00100 float Steering::purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
00101 {
00102   float SA;
00103 
00104   // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
00105   float Rx = (Bx - Ax);
00106   // compute run for prev wpt to bot; or compute vector offset by A(x,y)
00107   float Ry = (By - Ay);
00108   // dx is the run for the path
00109   float dx = Cx - Ax;
00110   // dy is the rise for the path
00111   float dy = Cy - Ay;
00112   // this is hypoteneuse length squared
00113   float ACd2 = dx*dx+dy*dy;
00114   // length of hyptoenuse
00115   float ACd = sqrtf( ACd2 );
00116   
00117   float Rd = Rx*dx + Ry*dy;  
00118   float t = Rd / ACd2;
00119   // nearest point on current segment
00120   float Nx = Ax + dx*t; 
00121   float Ny = Ay + dy*t;  
00122   // Cross track error
00123   float NBx = Nx-Bx;
00124   float NBy = Ny-By;
00125   float cte = sqrtf(NBx*NBx + NBy*NBy);
00126     float NGd;
00127 
00128   float myLookAhead;
00129   
00130   if (cte <= _intercept) {
00131     myLookAhead = _intercept;
00132   } else {
00133     myLookAhead = _intercept + cte;
00134   }
00135   
00136   NGd = sqrt( myLookAhead*myLookAhead - cte*cte );
00137   float Gx = NGd * dx/ACd + Nx;
00138   float Gy = NGd * dy/ACd + Ny;
00139   
00140   float hdgr = hdg*PI/180;
00141   
00142   float BGx = (Gx-Bx)*cos(hdgr) - (Gy-By)*sin(hdgr);
00143   float c = (2 * BGx) / (myLookAhead*myLookAhead);
00144 
00145   float radius;
00146   
00147   if (c != 0) {
00148     radius = 1/c;
00149   } else {
00150     radius = 999999.0;
00151   }
00152 
00153   // Now calculate steering angle based on wheelbase and track width
00154   SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
00155 
00156   return SA;
00157 }