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@1:cb84b477886c, 2013-05-28 (annotated)

Committer:
shimniok
Date:
Tue May 28 13:58:35 2013 +0000
Revision:
1:cb84b477886c
Parent:
0:a6a169de725f
Child:
2:fbc6e3cf3ed8
Changed initial next/prev waypoint to permit steering calc. Removed unnecessary code, added logging for steering angle to diagnose bug.

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