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@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?

UserRevisionLine numberNew 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 }