Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Actuators/Steering/Steering.cpp@0:a6a169de725f, 2013-05-27 (annotated)

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Child:
1:cb84b477886c
Working version with priorities set and update time display

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #include "Steering.h"
shimniok 0:a6a169de725f 2 #include "math.h"
shimniok 0:a6a169de725f 3
shimniok 0:a6a169de725f 4 /** create a new steering calculator for a particular vehicle
shimniok 0:a6a169de725f 5 *
shimniok 0:a6a169de725f 6 */
shimniok 0:a6a169de725f 7 Steering::Steering(float wheelbase, float track)
shimniok 0:a6a169de725f 8 : _wheelbase(wheelbase)
shimniok 0:a6a169de725f 9 , _track(track)
shimniok 0:a6a169de725f 10 , _intercept(2.0)
shimniok 0:a6a169de725f 11 {
shimniok 0:a6a169de725f 12 }
shimniok 0:a6a169de725f 13
shimniok 0:a6a169de725f 14 void Steering::setIntercept(float intercept)
shimniok 0:a6a169de725f 15 {
shimniok 0:a6a169de725f 16 _intercept = intercept;
shimniok 0:a6a169de725f 17 }
shimniok 0:a6a169de725f 18
shimniok 0:a6a169de725f 19 /** Calculate a steering angle based on relative bearing
shimniok 0:a6a169de725f 20 *
shimniok 0:a6a169de725f 21 */
shimniok 0:a6a169de725f 22 float Steering::calcSA(float theta)
shimniok 0:a6a169de725f 23 {
shimniok 0:a6a169de725f 24 return calcSA(theta, -1.0); // call with no limit
shimniok 0:a6a169de725f 25 }
shimniok 0:a6a169de725f 26
shimniok 0:a6a169de725f 27 /** calcSA
shimniok 0:a6a169de725f 28 * minRadius -- radius limit (minRadius < 0 disables limiting)
shimniok 0:a6a169de725f 29 */
shimniok 0:a6a169de725f 30 float Steering::calcSA(float theta, float minRadius)
shimniok 0:a6a169de725f 31 {
shimniok 0:a6a169de725f 32 float radius;
shimniok 0:a6a169de725f 33 float SA;
shimniok 0:a6a169de725f 34 bool neg = (theta < 0);
shimniok 0:a6a169de725f 35
shimniok 0:a6a169de725f 36 // I haven't had time to work out why the equation is slightly offset such
shimniok 0:a6a169de725f 37 // that negative angle produces slightly less steering angle
shimniok 0:a6a169de725f 38 //
shimniok 0:a6a169de725f 39 if (neg) theta = -theta;
shimniok 0:a6a169de725f 40
shimniok 0:a6a169de725f 41 // The equation peaks out at 90* so clamp theta artifically to 90, so that
shimniok 0:a6a169de725f 42 // if theta is actually > 90, we select max steering
shimniok 0:a6a169de725f 43 if (theta > 90.0) theta = 90.0;
shimniok 0:a6a169de725f 44
shimniok 0:a6a169de725f 45 // Compute |radius| based on intercept distance and specified angle with extra gain to
shimniok 0:a6a169de725f 46 // overcome steering slop, misalignment, sidehills, etc.
shimniok 0:a6a169de725f 47 radius = _intercept / ( 2 * sin(angle_radians(theta)) );
shimniok 0:a6a169de725f 48
shimniok 0:a6a169de725f 49 if (minRadius > 0) {
shimniok 0:a6a169de725f 50 if (radius < minRadius) radius = minRadius;
shimniok 0:a6a169de725f 51 }
shimniok 0:a6a169de725f 52
shimniok 0:a6a169de725f 53 // Now calculate steering angle based on wheelbase and track width
shimniok 0:a6a169de725f 54 SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 0:a6a169de725f 55 // The above ignores the effect of speed on required steering angle.
shimniok 0:a6a169de725f 56 // Even when under the limits of traction, understeer means more angle
shimniok 0:a6a169de725f 57 // is required to achieve a turn at higher speeds than lower speeds.
shimniok 0:a6a169de725f 58 // To consider this, we'd need to measure the understeer gradient of
shimniok 0:a6a169de725f 59 // the vehicle (thanks to Project240 for this insight) and include
shimniok 0:a6a169de725f 60 // that in the calculation.
shimniok 0:a6a169de725f 61
shimniok 0:a6a169de725f 62 if (neg) SA = -SA;
shimniok 0:a6a169de725f 63
shimniok 0:a6a169de725f 64 return SA;
shimniok 0:a6a169de725f 65 }
shimniok 0:a6a169de725f 66
shimniok 0:a6a169de725f 67 /**
shimniok 0:a6a169de725f 68 * Bxy - robot coordinates
shimniok 0:a6a169de725f 69 * Axy - previous waypoint coords
shimniok 0:a6a169de725f 70 * Cxy - next waypoint coords
shimniok 0:a6a169de725f 71 */
shimniok 0:a6a169de725f 72 float Steering::crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy)
shimniok 0:a6a169de725f 73 {
shimniok 0:a6a169de725f 74 // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:a6a169de725f 75 float Rx = (Bx - Ax);
shimniok 0:a6a169de725f 76 // compute run for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:a6a169de725f 77 float Ry = (By - Ay);
shimniok 0:a6a169de725f 78 // dx is the run for the path
shimniok 0:a6a169de725f 79 float dx = Cx - Ax;
shimniok 0:a6a169de725f 80 // dy is the rise for the path
shimniok 0:a6a169de725f 81 float dy = Cy - Ay;
shimniok 0:a6a169de725f 82 // this is hypoteneuse length squared
shimniok 0:a6a169de725f 83 float ACd2 = dx*dx+dy*dy;
shimniok 0:a6a169de725f 84 // length of hyptoenuse
shimniok 0:a6a169de725f 85 float ACd = sqrtf( ACd2 );
shimniok 0:a6a169de725f 86
shimniok 0:a6a169de725f 87 float Rd = Rx*dx + Ry*dy;
shimniok 0:a6a169de725f 88 float t = Rd / ACd2;
shimniok 0:a6a169de725f 89 // nearest point on current segment
shimniok 0:a6a169de725f 90 float Nx = Ax + dx*t;
shimniok 0:a6a169de725f 91 float Ny = Ay + dy*t;
shimniok 0:a6a169de725f 92 // Cross track error
shimniok 0:a6a169de725f 93 float NBx = Nx-Bx;
shimniok 0:a6a169de725f 94 float NBy = Ny-By;
shimniok 0:a6a169de725f 95 float cte = sqrtf(NBx*NBx + NBy*NBy);
shimniok 0:a6a169de725f 96
shimniok 0:a6a169de725f 97 return cte;
shimniok 0:a6a169de725f 98 }
shimniok 0:a6a169de725f 99
shimniok 0:a6a169de725f 100
shimniok 0:a6a169de725f 101 float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
shimniok 0:a6a169de725f 102 {
shimniok 0:a6a169de725f 103 float SA;
shimniok 0:a6a169de725f 104 // Leg vector
shimniok 0:a6a169de725f 105 float Lx = Cx - Ax;
shimniok 0:a6a169de725f 106 float Ly = Cy - Ay;
shimniok 0:a6a169de725f 107 // Robot vector
shimniok 0:a6a169de725f 108 float Rx = Bx - Ax;
shimniok 0:a6a169de725f 109 float Ry = Cy - Ay;
shimniok 0:a6a169de725f 110
shimniok 0:a6a169de725f 111 // Find the goal point, a projection of the bot vector onto the current leg, moved ahead
shimniok 0:a6a169de725f 112 // along the path by the lookahead distance
shimniok 0:a6a169de725f 113 float legLength = sqrtf(Lx*Lx + Ly*Ly); // ||L||
shimniok 0:a6a169de725f 114 float proj = (Lx*Rx + Ly*Ry)/legLength; // R dot L/||L||, projection magnitude, bot vector onto leg vector
shimniok 0:a6a169de725f 115 float LAx = (proj+_intercept)*Lx/legLength + Ax; // find projection point + lookahead, along leg
shimniok 0:a6a169de725f 116 float LAy = (proj+_intercept)*Ly/legLength + Ay;
shimniok 0:a6a169de725f 117 // Compute a circle that is tangential to bot heading and intercepts bot
shimniok 0:a6a169de725f 118 // and goal point (LAx,LAy), the intercept circle. Then compute the steering
shimniok 0:a6a169de725f 119 // angle to trace that circle.
shimniok 0:a6a169de725f 120 float brg = 180*atan2(LAx-Rx,LAy-Ry)/PI;
shimniok 0:a6a169de725f 121 // would be nice to add in some noise to heading info
shimniok 0:a6a169de725f 122 float relBrg = brg-hdg;
shimniok 0:a6a169de725f 123 if (relBrg < -180.0)
shimniok 0:a6a169de725f 124 relBrg += 360.0;
shimniok 0:a6a169de725f 125 if (relBrg >= 180.0)
shimniok 0:a6a169de725f 126 relBrg -= 360.0;
shimniok 0:a6a169de725f 127 // I haven't had time to work out why the equation is asymmetrical, that is,
shimniok 0:a6a169de725f 128 // negative angle produces slightly less steering angle. So instead, just use
shimniok 0:a6a169de725f 129 // absolute value and restore sign later.
shimniok 0:a6a169de725f 130 float sign = (relBrg < 0) ? -1 : 1;
shimniok 0:a6a169de725f 131 // The equation peaks out at 90* so clamp theta artifically to 90, so that
shimniok 0:a6a169de725f 132 // if theta is actually > 90, we select max steering
shimniok 0:a6a169de725f 133 if (relBrg > 90.0) relBrg = 90.0;
shimniok 0:a6a169de725f 134 // Compute radius based on intercept distance and specified angle
shimniok 0:a6a169de725f 135 float radius = _intercept/(2*sin(fabs(relBrg)*PI/180));
shimniok 0:a6a169de725f 136 // optionally, limit radius min/max
shimniok 0:a6a169de725f 137 // Now compute the steering angle to achieve the circle of
shimniok 0:a6a169de725f 138 // Steering angle is based on wheelbase and track width
shimniok 0:a6a169de725f 139 SA = sign * angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 0:a6a169de725f 140
shimniok 0:a6a169de725f 141 return SA;
shimniok 0:a6a169de725f 142 }
shimniok 0:a6a169de725f 143
shimniok 0:a6a169de725f 144
shimniok 0:a6a169de725f 145 float Steering::purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
shimniok 0:a6a169de725f 146 {
shimniok 0:a6a169de725f 147 float SA;
shimniok 0:a6a169de725f 148
shimniok 0:a6a169de725f 149 // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:a6a169de725f 150 float Rx = (Bx - Ax);
shimniok 0:a6a169de725f 151 // compute run for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:a6a169de725f 152 float Ry = (By - Ay);
shimniok 0:a6a169de725f 153 // dx is the run for the path
shimniok 0:a6a169de725f 154 float dx = Cx - Ax;
shimniok 0:a6a169de725f 155 // dy is the rise for the path
shimniok 0:a6a169de725f 156 float dy = Cy - Ay;
shimniok 0:a6a169de725f 157 // this is hypoteneuse length squared
shimniok 0:a6a169de725f 158 float ACd2 = dx*dx+dy*dy;
shimniok 0:a6a169de725f 159 // length of hyptoenuse
shimniok 0:a6a169de725f 160 float ACd = sqrtf( ACd2 );
shimniok 0:a6a169de725f 161
shimniok 0:a6a169de725f 162 float Rd = Rx*dx + Ry*dy;
shimniok 0:a6a169de725f 163 float t = Rd / ACd2;
shimniok 0:a6a169de725f 164 // nearest point on current segment
shimniok 0:a6a169de725f 165 float Nx = Ax + dx*t;
shimniok 0:a6a169de725f 166 float Ny = Ay + dy*t;
shimniok 0:a6a169de725f 167 // Cross track error
shimniok 0:a6a169de725f 168 float NBx = Nx-Bx;
shimniok 0:a6a169de725f 169 float NBy = Ny-By;
shimniok 0:a6a169de725f 170 float cte = sqrtf(NBx*NBx + NBy*NBy);
shimniok 0:a6a169de725f 171 float NGd;
shimniok 0:a6a169de725f 172
shimniok 0:a6a169de725f 173 float myLookAhead;
shimniok 0:a6a169de725f 174
shimniok 0:a6a169de725f 175 if (cte <= _intercept) {
shimniok 0:a6a169de725f 176 myLookAhead = _intercept;
shimniok 0:a6a169de725f 177 } else {
shimniok 0:a6a169de725f 178 myLookAhead = _intercept + cte;
shimniok 0:a6a169de725f 179 }
shimniok 0:a6a169de725f 180
shimniok 0:a6a169de725f 181 NGd = sqrt( myLookAhead*myLookAhead - cte*cte );
shimniok 0:a6a169de725f 182 float Gx = NGd * dx/ACd + Nx;
shimniok 0:a6a169de725f 183 float Gy = NGd * dy/ACd + Ny;
shimniok 0:a6a169de725f 184
shimniok 0:a6a169de725f 185 float hdgr = hdg*PI/180;
shimniok 0:a6a169de725f 186
shimniok 0:a6a169de725f 187 float BGx = (Gx-Bx)*cos(hdgr) - (Gy-By)*sin(hdgr);
shimniok 0:a6a169de725f 188 float c = (2 * BGx) / (myLookAhead*myLookAhead);
shimniok 0:a6a169de725f 189
shimniok 0:a6a169de725f 190 float radius;
shimniok 0:a6a169de725f 191
shimniok 0:a6a169de725f 192 if (c != 0) {
shimniok 0:a6a169de725f 193 radius = 1/c;
shimniok 0:a6a169de725f 194 } else {
shimniok 0:a6a169de725f 195 radius = 999999.0;
shimniok 0:a6a169de725f 196 }
shimniok 0:a6a169de725f 197
shimniok 0:a6a169de725f 198 // Now calculate steering angle based on wheelbase and track width
shimniok 0:a6a169de725f 199 SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 0:a6a169de725f 200
shimniok 0:a6a169de725f 201 return SA;
shimniok 0:a6a169de725f 202 }