Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Actuators/Steering/Steering.cpp@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:826c6171fc1b 1 #include "Steering.h"
shimniok 0:826c6171fc1b 2 #include "math.h"
shimniok 0:826c6171fc1b 3
shimniok 0:826c6171fc1b 4 /** create a new steering calculator for a particular vehicle
shimniok 0:826c6171fc1b 5 *
shimniok 0:826c6171fc1b 6 */
shimniok 0:826c6171fc1b 7 Steering::Steering(float wheelbase, float track)
shimniok 0:826c6171fc1b 8 : _wheelbase(wheelbase)
shimniok 0:826c6171fc1b 9 , _track(track)
shimniok 0:826c6171fc1b 10 , _intercept(2.0)
shimniok 0:826c6171fc1b 11 {
shimniok 0:826c6171fc1b 12 }
shimniok 0:826c6171fc1b 13
shimniok 0:826c6171fc1b 14 void Steering::setIntercept(float intercept)
shimniok 0:826c6171fc1b 15 {
shimniok 0:826c6171fc1b 16 _intercept = intercept;
shimniok 0:826c6171fc1b 17 }
shimniok 0:826c6171fc1b 18
shimniok 0:826c6171fc1b 19 /** Calculate a steering angle based on relative bearing
shimniok 0:826c6171fc1b 20 *
shimniok 0:826c6171fc1b 21 */
shimniok 0:826c6171fc1b 22 float Steering::calcSA(float theta) {
shimniok 0:826c6171fc1b 23 return calcSA(theta, -1.0); // call with no limit
shimniok 0:826c6171fc1b 24 }
shimniok 0:826c6171fc1b 25
shimniok 0:826c6171fc1b 26 /** calcSA
shimniok 0:826c6171fc1b 27 * minRadius -- radius limit (minRadius < 0 disables limiting)
shimniok 0:826c6171fc1b 28 */
shimniok 0:826c6171fc1b 29 float Steering::calcSA(float theta, float minRadius) {
shimniok 0:826c6171fc1b 30 float radius;
shimniok 0:826c6171fc1b 31 float SA;
shimniok 0:826c6171fc1b 32 bool neg = (theta < 0);
shimniok 0:826c6171fc1b 33
shimniok 0:826c6171fc1b 34 // I haven't had time to work out why the equation is slightly offset such
shimniok 0:826c6171fc1b 35 // that negative angle produces slightly less steering angle
shimniok 0:826c6171fc1b 36 //
shimniok 0:826c6171fc1b 37 if (neg) theta = -theta;
shimniok 0:826c6171fc1b 38
shimniok 0:826c6171fc1b 39 // The equation peaks out at 90* so clamp theta artifically to 90, so that
shimniok 0:826c6171fc1b 40 // if theta is actually > 90, we select max steering
shimniok 0:826c6171fc1b 41 if (theta > 90.0) theta = 90.0;
shimniok 0:826c6171fc1b 42
shimniok 0:826c6171fc1b 43 // Compute |radius| based on intercept distance and specified angle with extra gain to
shimniok 0:826c6171fc1b 44 // overcome steering slop, misalignment, sidehills, etc.
shimniok 0:826c6171fc1b 45 radius = _intercept / ( 2 * sin(angle_radians(theta)) );
shimniok 0:826c6171fc1b 46
shimniok 0:826c6171fc1b 47 if (minRadius > 0) {
shimniok 0:826c6171fc1b 48 if (radius < minRadius) radius = minRadius;
shimniok 0:826c6171fc1b 49 }
shimniok 0:826c6171fc1b 50
shimniok 0:826c6171fc1b 51 // Now calculate steering angle based on wheelbase and track width
shimniok 0:826c6171fc1b 52 SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 0:826c6171fc1b 53 // The above ignores the effect of speed on required steering angle.
shimniok 0:826c6171fc1b 54 // Even when under the limits of traction, understeer means more angle
shimniok 0:826c6171fc1b 55 // is required to achieve a turn at higher speeds than lower speeds.
shimniok 0:826c6171fc1b 56 // To consider this, we'd need to measure the understeer gradient of
shimniok 0:826c6171fc1b 57 // the vehicle (thanks to Project240 for this insight) and include
shimniok 0:826c6171fc1b 58 // that in the calculation.
shimniok 0:826c6171fc1b 59
shimniok 0:826c6171fc1b 60 if (neg) SA = -SA;
shimniok 0:826c6171fc1b 61
shimniok 0:826c6171fc1b 62 return SA;
shimniok 0:826c6171fc1b 63 }
shimniok 0:826c6171fc1b 64
shimniok 0:826c6171fc1b 65 /**
shimniok 0:826c6171fc1b 66 * Bxy - robot coordinates
shimniok 0:826c6171fc1b 67 * Axy - previous waypoint coords
shimniok 0:826c6171fc1b 68 * Cxy - next waypoint coords
shimniok 0:826c6171fc1b 69 */
shimniok 0:826c6171fc1b 70 float Steering::crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy)
shimniok 0:826c6171fc1b 71 {
shimniok 0:826c6171fc1b 72 // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:826c6171fc1b 73 float Rx = (Bx - Ax);
shimniok 0:826c6171fc1b 74 // compute run for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:826c6171fc1b 75 float Ry = (By - Ay);
shimniok 0:826c6171fc1b 76 // dx is the run for the path
shimniok 0:826c6171fc1b 77 float dx = Cx - Ax;
shimniok 0:826c6171fc1b 78 // dy is the rise for the path
shimniok 0:826c6171fc1b 79 float dy = Cy - Ay;
shimniok 0:826c6171fc1b 80 // this is hypoteneuse length squared
shimniok 0:826c6171fc1b 81 float ACd2 = dx*dx+dy*dy;
shimniok 0:826c6171fc1b 82 // length of hyptoenuse
shimniok 0:826c6171fc1b 83 float ACd = sqrtf( ACd2 );
shimniok 0:826c6171fc1b 84
shimniok 0:826c6171fc1b 85 float Rd = Rx*dx + Ry*dy;
shimniok 0:826c6171fc1b 86 float t = Rd / ACd2;
shimniok 0:826c6171fc1b 87 // nearest point on current segment
shimniok 0:826c6171fc1b 88 float Nx = Ax + dx*t;
shimniok 0:826c6171fc1b 89 float Ny = Ay + dy*t;
shimniok 0:826c6171fc1b 90 // Cross track error
shimniok 0:826c6171fc1b 91 float NBx = Nx-Bx;
shimniok 0:826c6171fc1b 92 float NBy = Ny-By;
shimniok 0:826c6171fc1b 93 float cte = sqrtf(NBx*NBx + NBy*NBy);
shimniok 0:826c6171fc1b 94
shimniok 0:826c6171fc1b 95 return cte;
shimniok 0:826c6171fc1b 96 }
shimniok 0:826c6171fc1b 97
shimniok 0:826c6171fc1b 98
shimniok 0:826c6171fc1b 99
shimniok 0:826c6171fc1b 100 float Steering::purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
shimniok 0:826c6171fc1b 101 {
shimniok 0:826c6171fc1b 102 float SA;
shimniok 0:826c6171fc1b 103
shimniok 0:826c6171fc1b 104 // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:826c6171fc1b 105 float Rx = (Bx - Ax);
shimniok 0:826c6171fc1b 106 // compute run for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:826c6171fc1b 107 float Ry = (By - Ay);
shimniok 0:826c6171fc1b 108 // dx is the run for the path
shimniok 0:826c6171fc1b 109 float dx = Cx - Ax;
shimniok 0:826c6171fc1b 110 // dy is the rise for the path
shimniok 0:826c6171fc1b 111 float dy = Cy - Ay;
shimniok 0:826c6171fc1b 112 // this is hypoteneuse length squared
shimniok 0:826c6171fc1b 113 float ACd2 = dx*dx+dy*dy;
shimniok 0:826c6171fc1b 114 // length of hyptoenuse
shimniok 0:826c6171fc1b 115 float ACd = sqrtf( ACd2 );
shimniok 0:826c6171fc1b 116
shimniok 0:826c6171fc1b 117 float Rd = Rx*dx + Ry*dy;
shimniok 0:826c6171fc1b 118 float t = Rd / ACd2;
shimniok 0:826c6171fc1b 119 // nearest point on current segment
shimniok 0:826c6171fc1b 120 float Nx = Ax + dx*t;
shimniok 0:826c6171fc1b 121 float Ny = Ay + dy*t;
shimniok 0:826c6171fc1b 122 // Cross track error
shimniok 0:826c6171fc1b 123 float NBx = Nx-Bx;
shimniok 0:826c6171fc1b 124 float NBy = Ny-By;
shimniok 0:826c6171fc1b 125 float cte = sqrtf(NBx*NBx + NBy*NBy);
shimniok 0:826c6171fc1b 126 float NGd;
shimniok 0:826c6171fc1b 127
shimniok 0:826c6171fc1b 128 float myLookAhead;
shimniok 0:826c6171fc1b 129
shimniok 0:826c6171fc1b 130 if (cte <= _intercept) {
shimniok 0:826c6171fc1b 131 myLookAhead = _intercept;
shimniok 0:826c6171fc1b 132 } else {
shimniok 0:826c6171fc1b 133 myLookAhead = _intercept + cte;
shimniok 0:826c6171fc1b 134 }
shimniok 0:826c6171fc1b 135
shimniok 0:826c6171fc1b 136 NGd = sqrt( myLookAhead*myLookAhead - cte*cte );
shimniok 0:826c6171fc1b 137 float Gx = NGd * dx/ACd + Nx;
shimniok 0:826c6171fc1b 138 float Gy = NGd * dy/ACd + Ny;
shimniok 0:826c6171fc1b 139
shimniok 0:826c6171fc1b 140 float hdgr = hdg*PI/180;
shimniok 0:826c6171fc1b 141
shimniok 0:826c6171fc1b 142 float BGx = (Gx-Bx)*cos(hdgr) - (Gy-By)*sin(hdgr);
shimniok 0:826c6171fc1b 143 float c = (2 * BGx) / (myLookAhead*myLookAhead);
shimniok 0:826c6171fc1b 144
shimniok 0:826c6171fc1b 145 float radius;
shimniok 0:826c6171fc1b 146
shimniok 0:826c6171fc1b 147 if (c != 0) {
shimniok 0:826c6171fc1b 148 radius = 1/c;
shimniok 0:826c6171fc1b 149 } else {
shimniok 0:826c6171fc1b 150 radius = 999999.0;
shimniok 0:826c6171fc1b 151 }
shimniok 0:826c6171fc1b 152
shimniok 0:826c6171fc1b 153 // Now calculate steering angle based on wheelbase and track width
shimniok 0:826c6171fc1b 154 SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 0:826c6171fc1b 155
shimniok 0:826c6171fc1b 156 return SA;
shimniok 0:826c6171fc1b 157 }