![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
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@25:bb5356402687, 2018-11-30 (annotated)
- Committer:
- shimniok
- Date:
- Fri Nov 30 16:11:53 2018 +0000
- Revision:
- 25:bb5356402687
- Parent:
- 15:01fb4916a5cd
Initial publish of revised version.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 3:42f3821c4e54 | 1 | #include "mbed.h" |
shimniok | 15:01fb4916a5cd | 2 | #include "globals.h" |
shimniok | 0:a6a169de725f | 3 | #include "Steering.h" |
shimniok | 0:a6a169de725f | 4 | #include "math.h" |
shimniok | 3:42f3821c4e54 | 5 | #include "util.h" |
shimniok | 1:cb84b477886c | 6 | |
shimniok | 15:01fb4916a5cd | 7 | Steering::Steering(PinName pin): |
shimniok | 15:01fb4916a5cd | 8 | _steering(pin), |
shimniok | 15:01fb4916a5cd | 9 | _scale(0), |
shimniok | 15:01fb4916a5cd | 10 | _track(0), |
shimniok | 15:01fb4916a5cd | 11 | _wheelbase(0), |
shimniok | 15:01fb4916a5cd | 12 | _intercept(0) |
shimniok | 0:a6a169de725f | 13 | { |
shimniok | 0:a6a169de725f | 14 | } |
shimniok | 0:a6a169de725f | 15 | |
shimniok | 15:01fb4916a5cd | 16 | |
shimniok | 15:01fb4916a5cd | 17 | void Steering::initSteering() |
shimniok | 0:a6a169de725f | 18 | { |
shimniok | 15:01fb4916a5cd | 19 | _steering = 0.4; |
shimniok | 15:01fb4916a5cd | 20 | // TODO: 3 parameterize this in config file ? |
shimniok | 15:01fb4916a5cd | 21 | _steering.calibrate(0.005, 45.0); |
shimniok | 15:01fb4916a5cd | 22 | } |
shimniok | 15:01fb4916a5cd | 23 | |
shimniok | 15:01fb4916a5cd | 24 | void Steering::setScale(float scale) { |
shimniok | 15:01fb4916a5cd | 25 | _scale = scale; |
shimniok | 15:01fb4916a5cd | 26 | } |
shimniok | 15:01fb4916a5cd | 27 | |
shimniok | 15:01fb4916a5cd | 28 | void Steering::setIntercept(float intercept) { |
shimniok | 0:a6a169de725f | 29 | _intercept = intercept; |
shimniok | 0:a6a169de725f | 30 | } |
shimniok | 0:a6a169de725f | 31 | |
shimniok | 15:01fb4916a5cd | 32 | void Steering::setWheelbase(float wheelbase) { |
shimniok | 15:01fb4916a5cd | 33 | _wheelbase = wheelbase; |
shimniok | 15:01fb4916a5cd | 34 | } |
shimniok | 15:01fb4916a5cd | 35 | |
shimniok | 15:01fb4916a5cd | 36 | void Steering::setTrack(float track) { |
shimniok | 15:01fb4916a5cd | 37 | _track = track; |
shimniok | 15:01fb4916a5cd | 38 | } |
shimniok | 15:01fb4916a5cd | 39 | |
shimniok | 15:01fb4916a5cd | 40 | // TODO 3 convert steering to Servo2 library |
shimniok | 15:01fb4916a5cd | 41 | void Steering::setSteering(float steerAngle) |
shimniok | 15:01fb4916a5cd | 42 | { |
shimniok | 15:01fb4916a5cd | 43 | // Convert steerAngle to servo value |
shimniok | 15:01fb4916a5cd | 44 | // Testing determined near linear conversion between servo ms setting and steering angle |
shimniok | 15:01fb4916a5cd | 45 | // up to 20*. Assumes a particular servo library with range = 0.005 |
shimniok | 15:01fb4916a5cd | 46 | // In that case, f(SA) = servoPosition = 0.500 + SA/762.5 |
shimniok | 15:01fb4916a5cd | 47 | // between 20 and 24* the slope is approximately 475 |
shimniok | 15:01fb4916a5cd | 48 | // What if we ignore the linearity and just set to a max angle |
shimniok | 15:01fb4916a5cd | 49 | // also range is 0.535-0.460 --> slope = 800 |
shimniok | 15:01fb4916a5cd | 50 | // steering = 0.500 + (double) steerAngle / 762.5; |
shimniok | 15:01fb4916a5cd | 51 | // |
shimniok | 15:01fb4916a5cd | 52 | _steering = 0.500 + (double) steerAngle * _scale; |
shimniok | 15:01fb4916a5cd | 53 | } |
shimniok | 15:01fb4916a5cd | 54 | |
shimniok | 15:01fb4916a5cd | 55 | |
shimniok | 15:01fb4916a5cd | 56 | float Steering::operator =(float steerAngle) |
shimniok | 15:01fb4916a5cd | 57 | { |
shimniok | 15:01fb4916a5cd | 58 | setSteering(steerAngle); |
shimniok | 15:01fb4916a5cd | 59 | return steerAngle; |
shimniok | 15:01fb4916a5cd | 60 | } |
shimniok | 15:01fb4916a5cd | 61 | |
shimniok | 15:01fb4916a5cd | 62 | |
shimniok | 0:a6a169de725f | 63 | /** Calculate a steering angle based on relative bearing |
shimniok | 0:a6a169de725f | 64 | * |
shimniok | 0:a6a169de725f | 65 | */ |
shimniok | 0:a6a169de725f | 66 | float Steering::calcSA(float theta) |
shimniok | 0:a6a169de725f | 67 | { |
shimniok | 0:a6a169de725f | 68 | return calcSA(theta, -1.0); // call with no limit |
shimniok | 0:a6a169de725f | 69 | } |
shimniok | 0:a6a169de725f | 70 | |
shimniok | 0:a6a169de725f | 71 | /** calcSA |
shimniok | 0:a6a169de725f | 72 | * minRadius -- radius limit (minRadius < 0 disables limiting) |
shimniok | 0:a6a169de725f | 73 | */ |
shimniok | 0:a6a169de725f | 74 | float Steering::calcSA(float theta, float minRadius) |
shimniok | 0:a6a169de725f | 75 | { |
shimniok | 0:a6a169de725f | 76 | float radius; |
shimniok | 0:a6a169de725f | 77 | float SA; |
shimniok | 0:a6a169de725f | 78 | bool neg = (theta < 0); |
shimniok | 0:a6a169de725f | 79 | |
shimniok | 0:a6a169de725f | 80 | // I haven't had time to work out why the equation is slightly offset such |
shimniok | 0:a6a169de725f | 81 | // that negative angle produces slightly less steering angle |
shimniok | 0:a6a169de725f | 82 | // |
shimniok | 0:a6a169de725f | 83 | if (neg) theta = -theta; |
shimniok | 0:a6a169de725f | 84 | |
shimniok | 0:a6a169de725f | 85 | // The equation peaks out at 90* so clamp theta artifically to 90, so that |
shimniok | 0:a6a169de725f | 86 | // if theta is actually > 90, we select max steering |
shimniok | 0:a6a169de725f | 87 | if (theta > 90.0) theta = 90.0; |
shimniok | 0:a6a169de725f | 88 | |
shimniok | 0:a6a169de725f | 89 | // Compute |radius| based on intercept distance and specified angle with extra gain to |
shimniok | 0:a6a169de725f | 90 | // overcome steering slop, misalignment, sidehills, etc. |
shimniok | 15:01fb4916a5cd | 91 | radius = _intercept / ( 2 * sin(toRadians(theta)) ); |
shimniok | 0:a6a169de725f | 92 | |
shimniok | 0:a6a169de725f | 93 | if (minRadius > 0) { |
shimniok | 0:a6a169de725f | 94 | if (radius < minRadius) radius = minRadius; |
shimniok | 0:a6a169de725f | 95 | } |
shimniok | 0:a6a169de725f | 96 | |
shimniok | 0:a6a169de725f | 97 | // Now calculate steering angle based on wheelbase and track width |
shimniok | 15:01fb4916a5cd | 98 | SA = toDegrees(asin(_wheelbase / (radius - _track/2))); |
shimniok | 0:a6a169de725f | 99 | // The above ignores the effect of speed on required steering angle. |
shimniok | 0:a6a169de725f | 100 | // Even when under the limits of traction, understeer means more angle |
shimniok | 0:a6a169de725f | 101 | // is required to achieve a turn at higher speeds than lower speeds. |
shimniok | 0:a6a169de725f | 102 | // To consider this, we'd need to measure the understeer gradient of |
shimniok | 0:a6a169de725f | 103 | // the vehicle (thanks to Project240 for this insight) and include |
shimniok | 0:a6a169de725f | 104 | // that in the calculation. |
shimniok | 0:a6a169de725f | 105 | |
shimniok | 0:a6a169de725f | 106 | if (neg) SA = -SA; |
shimniok | 0:a6a169de725f | 107 | |
shimniok | 0:a6a169de725f | 108 | return SA; |
shimniok | 0:a6a169de725f | 109 | } |
shimniok | 0:a6a169de725f | 110 | |
shimniok | 0:a6a169de725f | 111 | /** |
shimniok | 0:a6a169de725f | 112 | * Bxy - robot coordinates |
shimniok | 0:a6a169de725f | 113 | * Axy - previous waypoint coords |
shimniok | 0:a6a169de725f | 114 | * Cxy - next waypoint coords |
shimniok | 0:a6a169de725f | 115 | */ |
shimniok | 0:a6a169de725f | 116 | float Steering::crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy) |
shimniok | 0:a6a169de725f | 117 | { |
shimniok | 1:cb84b477886c | 118 | /* |
shimniok | 0:a6a169de725f | 119 | // Compute rise for prev wpt to bot; or compute vector offset by A(x,y) |
shimniok | 0:a6a169de725f | 120 | float Rx = (Bx - Ax); |
shimniok | 0:a6a169de725f | 121 | // compute run for prev wpt to bot; or compute vector offset by A(x,y) |
shimniok | 0:a6a169de725f | 122 | float Ry = (By - Ay); |
shimniok | 0:a6a169de725f | 123 | // dx is the run for the path |
shimniok | 0:a6a169de725f | 124 | float dx = Cx - Ax; |
shimniok | 0:a6a169de725f | 125 | // dy is the rise for the path |
shimniok | 0:a6a169de725f | 126 | float dy = Cy - Ay; |
shimniok | 0:a6a169de725f | 127 | // this is hypoteneuse length squared |
shimniok | 0:a6a169de725f | 128 | float ACd2 = dx*dx+dy*dy; |
shimniok | 0:a6a169de725f | 129 | // length of hyptoenuse |
shimniok | 0:a6a169de725f | 130 | float ACd = sqrtf( ACd2 ); |
shimniok | 0:a6a169de725f | 131 | |
shimniok | 0:a6a169de725f | 132 | float Rd = Rx*dx + Ry*dy; |
shimniok | 0:a6a169de725f | 133 | float t = Rd / ACd2; |
shimniok | 0:a6a169de725f | 134 | // nearest point on current segment |
shimniok | 0:a6a169de725f | 135 | float Nx = Ax + dx*t; |
shimniok | 0:a6a169de725f | 136 | float Ny = Ay + dy*t; |
shimniok | 0:a6a169de725f | 137 | // Cross track error |
shimniok | 0:a6a169de725f | 138 | float NBx = Nx-Bx; |
shimniok | 0:a6a169de725f | 139 | float NBy = Ny-By; |
shimniok | 0:a6a169de725f | 140 | float cte = sqrtf(NBx*NBx + NBy*NBy); |
shimniok | 0:a6a169de725f | 141 | return cte; |
shimniok | 1:cb84b477886c | 142 | */ |
shimniok | 1:cb84b477886c | 143 | return 0; |
shimniok | 0:a6a169de725f | 144 | } |
shimniok | 0:a6a169de725f | 145 | |
shimniok | 2:fbc6e3cf3ed8 | 146 | //static int skip=0; |
shimniok | 0:a6a169de725f | 147 | |
shimniok | 0:a6a169de725f | 148 | float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy) |
shimniok | 0:a6a169de725f | 149 | { |
shimniok | 0:a6a169de725f | 150 | // Leg vector |
shimniok | 0:a6a169de725f | 151 | float Lx = Cx - Ax; |
shimniok | 0:a6a169de725f | 152 | float Ly = Cy - Ay; |
shimniok | 0:a6a169de725f | 153 | // Robot vector |
shimniok | 0:a6a169de725f | 154 | float Rx = Bx - Ax; |
shimniok | 1:cb84b477886c | 155 | float Ry = By - Ay; |
shimniok | 3:42f3821c4e54 | 156 | float sign = 1; |
shimniok | 0:a6a169de725f | 157 | |
shimniok | 0:a6a169de725f | 158 | // Find the goal point, a projection of the bot vector onto the current leg, moved ahead |
shimniok | 0:a6a169de725f | 159 | // along the path by the lookahead distance |
shimniok | 0:a6a169de725f | 160 | float legLength = sqrtf(Lx*Lx + Ly*Ly); // ||L|| |
shimniok | 0:a6a169de725f | 161 | float proj = (Lx*Rx + Ly*Ry)/legLength; // R dot L/||L||, projection magnitude, bot vector onto leg vector |
shimniok | 2:fbc6e3cf3ed8 | 162 | float LAx = (proj + _intercept)*Lx/legLength; // find projection point + lookahead, along leg, relative to Bx |
shimniok | 2:fbc6e3cf3ed8 | 163 | float LAy = (proj + _intercept)*Ly/legLength; |
shimniok | 0:a6a169de725f | 164 | // Compute a circle that is tangential to bot heading and intercepts bot |
shimniok | 0:a6a169de725f | 165 | // and goal point (LAx,LAy), the intercept circle. Then compute the steering |
shimniok | 1:cb84b477886c | 166 | // angle to trace that circle. (x,y because 0 deg points up not right) |
shimniok | 15:01fb4916a5cd | 167 | float brg = clamp360( toDegrees(atan2(LAx-Rx,LAy-Ry)) ); |
shimniok | 3:42f3821c4e54 | 168 | //if (brg >= 360.0) brg -= 360.0; |
shimniok | 3:42f3821c4e54 | 169 | //if (brg < 0) brg += 360.0; |
shimniok | 0:a6a169de725f | 170 | // would be nice to add in some noise to heading info |
shimniok | 3:42f3821c4e54 | 171 | float relBrg = clamp180(brg - hdg); |
shimniok | 3:42f3821c4e54 | 172 | // The steering angle equation actually peaks at relBrg == 90 so just clamp to this |
shimniok | 3:42f3821c4e54 | 173 | if (relBrg > 89.5) { |
shimniok | 3:42f3821c4e54 | 174 | relBrg = 89.5; |
shimniok | 3:42f3821c4e54 | 175 | } else if (relBrg < -89.5) { |
shimniok | 3:42f3821c4e54 | 176 | relBrg = -89.5; |
shimniok | 3:42f3821c4e54 | 177 | } |
shimniok | 0:a6a169de725f | 178 | // Compute radius based on intercept distance and specified angle |
shimniok | 3:42f3821c4e54 | 179 | // The sin equation will produce a negative radius, which causes problems |
shimniok | 3:42f3821c4e54 | 180 | // when subtracting track/2.0, so just take absolute value and multiply sign |
shimniok | 3:42f3821c4e54 | 181 | // later on |
shimniok | 3:42f3821c4e54 | 182 | sign = (relBrg < 0) ? -1 : 1; |
shimniok | 15:01fb4916a5cd | 183 | float radius = _intercept/fabs(2*sin(toRadians(relBrg))); |
shimniok | 0:a6a169de725f | 184 | // optionally, limit radius min/max |
shimniok | 0:a6a169de725f | 185 | // Now compute the steering angle to achieve the circle of |
shimniok | 0:a6a169de725f | 186 | // Steering angle is based on wheelbase and track width |
shimniok | 15:01fb4916a5cd | 187 | return ( sign * toDegrees(asin(_wheelbase / (radius - _track/2.0))) ); |
shimniok | 0:a6a169de725f | 188 | } |
shimniok | 0:a6a169de725f | 189 | |
shimniok | 0:a6a169de725f | 190 | |
shimniok | 0:a6a169de725f | 191 | float Steering::purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy) |
shimniok | 0:a6a169de725f | 192 | { |
shimniok | 0:a6a169de725f | 193 | float SA; |
shimniok | 0:a6a169de725f | 194 | |
shimniok | 0:a6a169de725f | 195 | // Compute rise for prev wpt to bot; or compute vector offset by A(x,y) |
shimniok | 0:a6a169de725f | 196 | float Rx = (Bx - Ax); |
shimniok | 0:a6a169de725f | 197 | // compute run for prev wpt to bot; or compute vector offset by A(x,y) |
shimniok | 0:a6a169de725f | 198 | float Ry = (By - Ay); |
shimniok | 0:a6a169de725f | 199 | // dx is the run for the path |
shimniok | 0:a6a169de725f | 200 | float dx = Cx - Ax; |
shimniok | 0:a6a169de725f | 201 | // dy is the rise for the path |
shimniok | 0:a6a169de725f | 202 | float dy = Cy - Ay; |
shimniok | 0:a6a169de725f | 203 | // this is hypoteneuse length squared |
shimniok | 0:a6a169de725f | 204 | float ACd2 = dx*dx+dy*dy; |
shimniok | 0:a6a169de725f | 205 | // length of hyptoenuse |
shimniok | 0:a6a169de725f | 206 | float ACd = sqrtf( ACd2 ); |
shimniok | 0:a6a169de725f | 207 | |
shimniok | 0:a6a169de725f | 208 | float Rd = Rx*dx + Ry*dy; |
shimniok | 0:a6a169de725f | 209 | float t = Rd / ACd2; |
shimniok | 0:a6a169de725f | 210 | // nearest point on current segment |
shimniok | 0:a6a169de725f | 211 | float Nx = Ax + dx*t; |
shimniok | 0:a6a169de725f | 212 | float Ny = Ay + dy*t; |
shimniok | 0:a6a169de725f | 213 | // Cross track error |
shimniok | 0:a6a169de725f | 214 | float NBx = Nx-Bx; |
shimniok | 0:a6a169de725f | 215 | float NBy = Ny-By; |
shimniok | 0:a6a169de725f | 216 | float cte = sqrtf(NBx*NBx + NBy*NBy); |
shimniok | 0:a6a169de725f | 217 | float NGd; |
shimniok | 0:a6a169de725f | 218 | |
shimniok | 0:a6a169de725f | 219 | float myLookAhead; |
shimniok | 0:a6a169de725f | 220 | |
shimniok | 0:a6a169de725f | 221 | if (cte <= _intercept) { |
shimniok | 0:a6a169de725f | 222 | myLookAhead = _intercept; |
shimniok | 0:a6a169de725f | 223 | } else { |
shimniok | 0:a6a169de725f | 224 | myLookAhead = _intercept + cte; |
shimniok | 0:a6a169de725f | 225 | } |
shimniok | 0:a6a169de725f | 226 | |
shimniok | 0:a6a169de725f | 227 | NGd = sqrt( myLookAhead*myLookAhead - cte*cte ); |
shimniok | 0:a6a169de725f | 228 | float Gx = NGd * dx/ACd + Nx; |
shimniok | 0:a6a169de725f | 229 | float Gy = NGd * dy/ACd + Ny; |
shimniok | 0:a6a169de725f | 230 | |
shimniok | 0:a6a169de725f | 231 | float hdgr = hdg*PI/180; |
shimniok | 0:a6a169de725f | 232 | |
shimniok | 0:a6a169de725f | 233 | float BGx = (Gx-Bx)*cos(hdgr) - (Gy-By)*sin(hdgr); |
shimniok | 0:a6a169de725f | 234 | float c = (2 * BGx) / (myLookAhead*myLookAhead); |
shimniok | 0:a6a169de725f | 235 | |
shimniok | 0:a6a169de725f | 236 | float radius; |
shimniok | 0:a6a169de725f | 237 | |
shimniok | 0:a6a169de725f | 238 | if (c != 0) { |
shimniok | 0:a6a169de725f | 239 | radius = 1/c; |
shimniok | 0:a6a169de725f | 240 | } else { |
shimniok | 0:a6a169de725f | 241 | radius = 999999.0; |
shimniok | 0:a6a169de725f | 242 | } |
shimniok | 0:a6a169de725f | 243 | |
shimniok | 0:a6a169de725f | 244 | // Now calculate steering angle based on wheelbase and track width |
shimniok | 15:01fb4916a5cd | 245 | SA = toDegrees(asin(_wheelbase / (radius - _track/2))); |
shimniok | 0:a6a169de725f | 246 | |
shimniok | 0:a6a169de725f | 247 | return SA; |
shimniok | 15:01fb4916a5cd | 248 | } |