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@2:fbc6e3cf3ed8, 2013-06-06 (annotated)

Committer:
shimniok
Date:
Thu Jun 06 13:40:23 2013 +0000
Revision:
2:fbc6e3cf3ed8
Parent:
1:cb84b477886c
Child:
3:42f3821c4e54
Sort-of working version, still some errors with estimation. Added clamp() function.

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 2:fbc6e3cf3ed8 9 Steering::Steering(float wheelbase, float track):
shimniok 2:fbc6e3cf3ed8 10 _wheelbase(wheelbase),
shimniok 2:fbc6e3cf3ed8 11 _track(track),
shimniok 2:fbc6e3cf3ed8 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 2:fbc6e3cf3ed8 104 //static int skip=0;
shimniok 0:a6a169de725f 105
shimniok 0:a6a169de725f 106 float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
shimniok 0:a6a169de725f 107 {
shimniok 0:a6a169de725f 108 float SA;
shimniok 0:a6a169de725f 109 // Leg vector
shimniok 0:a6a169de725f 110 float Lx = Cx - Ax;
shimniok 0:a6a169de725f 111 float Ly = Cy - Ay;
shimniok 0:a6a169de725f 112 // Robot vector
shimniok 0:a6a169de725f 113 float Rx = Bx - Ax;
shimniok 1:cb84b477886c 114 float Ry = By - Ay;
shimniok 0:a6a169de725f 115
shimniok 0:a6a169de725f 116 // Find the goal point, a projection of the bot vector onto the current leg, moved ahead
shimniok 0:a6a169de725f 117 // along the path by the lookahead distance
shimniok 0:a6a169de725f 118 float legLength = sqrtf(Lx*Lx + Ly*Ly); // ||L||
shimniok 0:a6a169de725f 119 float proj = (Lx*Rx + Ly*Ry)/legLength; // R dot L/||L||, projection magnitude, bot vector onto leg vector
shimniok 2:fbc6e3cf3ed8 120 float LAx = (proj + _intercept)*Lx/legLength; // find projection point + lookahead, along leg, relative to Bx
shimniok 2:fbc6e3cf3ed8 121 float LAy = (proj + _intercept)*Ly/legLength;
shimniok 0:a6a169de725f 122 // Compute a circle that is tangential to bot heading and intercepts bot
shimniok 0:a6a169de725f 123 // and goal point (LAx,LAy), the intercept circle. Then compute the steering
shimniok 1:cb84b477886c 124 // angle to trace that circle. (x,y because 0 deg points up not right)
shimniok 2:fbc6e3cf3ed8 125 float brg = angle_degrees(atan2(LAx-Rx,LAy-Ry));
shimniok 2:fbc6e3cf3ed8 126 if (brg >= 360.0) brg -= 360.0;
shimniok 2:fbc6e3cf3ed8 127 if (brg < 0) brg += 360.0;
shimniok 0:a6a169de725f 128 // would be nice to add in some noise to heading info
shimniok 2:fbc6e3cf3ed8 129 float relBrg = brg - hdg;
shimniok 1:cb84b477886c 130 if (relBrg < -180.0) relBrg += 360.0;
shimniok 1:cb84b477886c 131 if (relBrg >= 180.0) relBrg -= 360.0;
shimniok 0:a6a169de725f 132 // The equation peaks out at 90* so clamp theta artifically to 90, so that
shimniok 0:a6a169de725f 133 // if theta is actually > 90, we select max steering
shimniok 1:cb84b477886c 134 if (fabs(relBrg) > 90.0) relBrg = 90.0;
shimniok 0:a6a169de725f 135 // Compute radius based on intercept distance and specified angle
shimniok 2:fbc6e3cf3ed8 136 float radius = _intercept/(2*sin(angle_radians(fabs(relBrg))));
shimniok 0:a6a169de725f 137 // optionally, limit radius min/max
shimniok 0:a6a169de725f 138 // Now compute the steering angle to achieve the circle of
shimniok 0:a6a169de725f 139 // Steering angle is based on wheelbase and track width
shimniok 2:fbc6e3cf3ed8 140 SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 2:fbc6e3cf3ed8 141 // I haven't had time to work out why the equation is asymmetrical, that is,
shimniok 2:fbc6e3cf3ed8 142 // negative angle produces slightly less steering angle.
shimniok 2:fbc6e3cf3ed8 143 if (relBrg < 0) SA = -SA;
shimniok 2:fbc6e3cf3ed8 144
shimniok 2:fbc6e3cf3ed8 145 /*
shimniok 2:fbc6e3cf3ed8 146 if (++skip >= 20) {
shimniok 2:fbc6e3cf3ed8 147 fprintf(stdout, "R(%.4f,%.4f) A(%.4f,%.4f) C(%.4f,%.4f) LA(%.4f,%.4f) h=%.2f b=%.2f rb=%.2f sa=%.2f\n", Bx, By, Ax, Ay, Cx, Cy, LAx, LAy, hdg, brg, relBrg, SA);
shimniok 2:fbc6e3cf3ed8 148 skip = 0;
shimniok 2:fbc6e3cf3ed8 149 }
shimniok 2:fbc6e3cf3ed8 150 */
shimniok 1:cb84b477886c 151
shimniok 0:a6a169de725f 152 return SA;
shimniok 0:a6a169de725f 153 }
shimniok 0:a6a169de725f 154
shimniok 0:a6a169de725f 155
shimniok 0:a6a169de725f 156 float Steering::purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
shimniok 0:a6a169de725f 157 {
shimniok 0:a6a169de725f 158 float SA;
shimniok 0:a6a169de725f 159
shimniok 0:a6a169de725f 160 // Compute rise for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:a6a169de725f 161 float Rx = (Bx - Ax);
shimniok 0:a6a169de725f 162 // compute run for prev wpt to bot; or compute vector offset by A(x,y)
shimniok 0:a6a169de725f 163 float Ry = (By - Ay);
shimniok 0:a6a169de725f 164 // dx is the run for the path
shimniok 0:a6a169de725f 165 float dx = Cx - Ax;
shimniok 0:a6a169de725f 166 // dy is the rise for the path
shimniok 0:a6a169de725f 167 float dy = Cy - Ay;
shimniok 0:a6a169de725f 168 // this is hypoteneuse length squared
shimniok 0:a6a169de725f 169 float ACd2 = dx*dx+dy*dy;
shimniok 0:a6a169de725f 170 // length of hyptoenuse
shimniok 0:a6a169de725f 171 float ACd = sqrtf( ACd2 );
shimniok 0:a6a169de725f 172
shimniok 0:a6a169de725f 173 float Rd = Rx*dx + Ry*dy;
shimniok 0:a6a169de725f 174 float t = Rd / ACd2;
shimniok 0:a6a169de725f 175 // nearest point on current segment
shimniok 0:a6a169de725f 176 float Nx = Ax + dx*t;
shimniok 0:a6a169de725f 177 float Ny = Ay + dy*t;
shimniok 0:a6a169de725f 178 // Cross track error
shimniok 0:a6a169de725f 179 float NBx = Nx-Bx;
shimniok 0:a6a169de725f 180 float NBy = Ny-By;
shimniok 0:a6a169de725f 181 float cte = sqrtf(NBx*NBx + NBy*NBy);
shimniok 0:a6a169de725f 182 float NGd;
shimniok 0:a6a169de725f 183
shimniok 0:a6a169de725f 184 float myLookAhead;
shimniok 0:a6a169de725f 185
shimniok 0:a6a169de725f 186 if (cte <= _intercept) {
shimniok 0:a6a169de725f 187 myLookAhead = _intercept;
shimniok 0:a6a169de725f 188 } else {
shimniok 0:a6a169de725f 189 myLookAhead = _intercept + cte;
shimniok 0:a6a169de725f 190 }
shimniok 0:a6a169de725f 191
shimniok 0:a6a169de725f 192 NGd = sqrt( myLookAhead*myLookAhead - cte*cte );
shimniok 0:a6a169de725f 193 float Gx = NGd * dx/ACd + Nx;
shimniok 0:a6a169de725f 194 float Gy = NGd * dy/ACd + Ny;
shimniok 0:a6a169de725f 195
shimniok 0:a6a169de725f 196 float hdgr = hdg*PI/180;
shimniok 0:a6a169de725f 197
shimniok 0:a6a169de725f 198 float BGx = (Gx-Bx)*cos(hdgr) - (Gy-By)*sin(hdgr);
shimniok 0:a6a169de725f 199 float c = (2 * BGx) / (myLookAhead*myLookAhead);
shimniok 0:a6a169de725f 200
shimniok 0:a6a169de725f 201 float radius;
shimniok 0:a6a169de725f 202
shimniok 0:a6a169de725f 203 if (c != 0) {
shimniok 0:a6a169de725f 204 radius = 1/c;
shimniok 0:a6a169de725f 205 } else {
shimniok 0:a6a169de725f 206 radius = 999999.0;
shimniok 0:a6a169de725f 207 }
shimniok 0:a6a169de725f 208
shimniok 0:a6a169de725f 209 // Now calculate steering angle based on wheelbase and track width
shimniok 0:a6a169de725f 210 SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
shimniok 0:a6a169de725f 211
shimniok 0:a6a169de725f 212 return SA;
shimniok 0:a6a169de725f 213 }