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@3:42f3821c4e54, 2013-06-07 (annotated)

Committer:
shimniok
Date:
Fri Jun 07 14:45:46 2013 +0000
Revision:
3:42f3821c4e54
Parent:
2:fbc6e3cf3ed8
Child:
15:01fb4916a5cd
Working version 6/6/2013. Heading estimation may not be quite perfect yet but seems the major estimation bugs are fixed now.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 3:42f3821c4e54 1 #include "mbed.h"
shimniok 0:a6a169de725f 2 #include "Steering.h"
shimniok 0:a6a169de725f 3 #include "math.h"
shimniok 3:42f3821c4e54 4 #include "util.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 // 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 3:42f3821c4e54 114 float sign = 1;
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 3:42f3821c4e54 125 float brg = clamp360( angle_degrees(atan2(LAx-Rx,LAy-Ry)) );
shimniok 3:42f3821c4e54 126 //if (brg >= 360.0) brg -= 360.0;
shimniok 3:42f3821c4e54 127 //if (brg < 0) brg += 360.0;
shimniok 0:a6a169de725f 128 // would be nice to add in some noise to heading info
shimniok 3:42f3821c4e54 129 float relBrg = clamp180(brg - hdg);
shimniok 3:42f3821c4e54 130 // The steering angle equation actually peaks at relBrg == 90 so just clamp to this
shimniok 3:42f3821c4e54 131 if (relBrg > 89.5) {
shimniok 3:42f3821c4e54 132 relBrg = 89.5;
shimniok 3:42f3821c4e54 133 } else if (relBrg < -89.5) {
shimniok 3:42f3821c4e54 134 relBrg = -89.5;
shimniok 3:42f3821c4e54 135 }
shimniok 0:a6a169de725f 136 // Compute radius based on intercept distance and specified angle
shimniok 3:42f3821c4e54 137 // The sin equation will produce a negative radius, which causes problems
shimniok 3:42f3821c4e54 138 // when subtracting track/2.0, so just take absolute value and multiply sign
shimniok 3:42f3821c4e54 139 // later on
shimniok 3:42f3821c4e54 140 sign = (relBrg < 0) ? -1 : 1;
shimniok 3:42f3821c4e54 141 float radius = _intercept/fabs(2*sin(angle_radians(relBrg)));
shimniok 0:a6a169de725f 142 // optionally, limit radius min/max
shimniok 0:a6a169de725f 143 // Now compute the steering angle to achieve the circle of
shimniok 0:a6a169de725f 144 // Steering angle is based on wheelbase and track width
shimniok 3:42f3821c4e54 145 return ( sign * angle_degrees(asin(_wheelbase / (radius - _track/2.0))) );
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 }