File content as of revision 0:826c6171fc1b:

#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;
}