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
Revision 3:42f3821c4e54, committed 2013-06-07
- Comitter:
- shimniok
- Date:
- Fri Jun 07 14:45:46 2013 +0000
- Parent:
- 2:fbc6e3cf3ed8
- Child:
- 4:2567b5292249
- Commit message:
- Working version 6/6/2013. Heading estimation may not be quite perfect yet but seems the major estimation bugs are fixed now.
Changed in this revision
--- a/Actuators/Actuators.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/Actuators/Actuators.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -12,8 +12,12 @@ void initSteering() { - // Setup steering servo - steering = config.steerZero; + if (config.loaded) { + // Setup steering servo + steering = config.steerZero; + } else { + steering = 0.4; + } // TODO: 3 parameterize this in config file steering.calibrate(0.005, 45.0); } @@ -21,7 +25,11 @@ void initThrottle() { - throttle = (float)config.escZero/1000.0; + if (config.loaded) { + throttle = (float)config.escZero/1000.0; + } else { + throttle = 0.410; + } // TODO: 3 parameterize this in config file throttle.calibrate(0.001, 45.0); }
--- a/Actuators/Steering/Steering.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/Actuators/Steering/Steering.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -1,7 +1,7 @@ +#include "mbed.h" #include "Steering.h" #include "math.h" - -#include "mbed.h" +#include "util.h" /** create a new steering calculator for a particular vehicle * @@ -105,13 +105,13 @@ float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy) { - float SA; // Leg vector float Lx = Cx - Ax; float Ly = Cy - Ay; // Robot vector float Rx = Bx - Ax; float Ry = By - Ay; + float sign = 1; // Find the goal point, a projection of the bot vector onto the current leg, moved ahead // along the path by the lookahead distance @@ -122,34 +122,27 @@ // Compute a circle that is tangential to bot heading and intercepts bot // and goal point (LAx,LAy), the intercept circle. Then compute the steering // angle to trace that circle. (x,y because 0 deg points up not right) - float brg = angle_degrees(atan2(LAx-Rx,LAy-Ry)); - if (brg >= 360.0) brg -= 360.0; - if (brg < 0) brg += 360.0; + float brg = clamp360( angle_degrees(atan2(LAx-Rx,LAy-Ry)) ); + //if (brg >= 360.0) brg -= 360.0; + //if (brg < 0) brg += 360.0; // would be nice to add in some noise to heading info - float relBrg = brg - hdg; - if (relBrg < -180.0) relBrg += 360.0; - if (relBrg >= 180.0) relBrg -= 360.0; - // The equation peaks out at 90* so clamp theta artifically to 90, so that - // if theta is actually > 90, we select max steering - if (fabs(relBrg) > 90.0) relBrg = 90.0; + float relBrg = clamp180(brg - hdg); + // The steering angle equation actually peaks at relBrg == 90 so just clamp to this + if (relBrg > 89.5) { + relBrg = 89.5; + } else if (relBrg < -89.5) { + relBrg = -89.5; + } // Compute radius based on intercept distance and specified angle - float radius = _intercept/(2*sin(angle_radians(fabs(relBrg)))); + // The sin equation will produce a negative radius, which causes problems + // when subtracting track/2.0, so just take absolute value and multiply sign + // later on + sign = (relBrg < 0) ? -1 : 1; + float radius = _intercept/fabs(2*sin(angle_radians(relBrg))); // optionally, limit radius min/max // Now compute the steering angle to achieve the circle of // Steering angle is based on wheelbase and track width - SA = angle_degrees(asin(_wheelbase / (radius - _track/2))); - // I haven't had time to work out why the equation is asymmetrical, that is, - // negative angle produces slightly less steering angle. - if (relBrg < 0) SA = -SA; - - /* - if (++skip >= 20) { - 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); - skip = 0; - } - */ - - return SA; + return ( sign * angle_degrees(asin(_wheelbase / (radius - _track/2.0))) ); }
--- a/Config/Config.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/Config/Config.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -10,7 +10,8 @@ // TODO: 3: mod waypoints to include speed after waypoint -Config::Config() +Config::Config(): + loaded(false) { // not much to do here, yet. } @@ -143,5 +144,7 @@ if (fp != 0) fclose(fp); + loaded = confStatus; + return confStatus; }
--- a/Config/Config.h Thu Jun 06 13:40:23 2013 +0000 +++ b/Config/Config.h Fri Jun 07 14:45:46 2013 +0000 @@ -11,7 +11,7 @@ Config(); bool load(); - + bool loaded; // has the config been loaded yet? float interceptDist; // used for course correction steering calculation float waypointDist; // distance threshold to waypoint float brakeDist; // braking distance
--- a/Estimation/kalman.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/Estimation/kalman.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -1,9 +1,10 @@ #include "mbed.h" #include "Matrix.h" +#include "util.h" #define DEBUG 1 -#define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360)) +//#define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360)) /* * Kalman Filter Setup @@ -176,17 +177,14 @@ ***********************************************************************/ float Hx[2]; Matrix_Multiply(2,2,1, Hx, H, xp); - + //Matrix_print(2,2, H, "6. H"); //Matrix_print(2,1, x, "6. x"); //Matrix_print(2,1, Hx, "6. Hx"); float zHx[2]; Matrix_Subtract(2,1, zHx, z, Hx); - - // At this point we need to be sure to correct heading to -180 to 180 range - if (zHx[0] > 180.0) zHx[0] -= 360.0; - if (zHx[0] <= -180.0) zHx[0] += 360.0; + zHx[0] = clamp180(zHx[0]); //Matrix_print(2,1, z, "6. z"); //Matrix_print(2,1, zHx, "6. zHx"); @@ -198,10 +196,7 @@ //Matrix_print(2,1, KzHx, "6. KzHx"); Matrix_Add(2,1, x, xp, KzHx); - - // Clamp to 0-360 range - while (x[0] < 0) x[0] += 360.0; - while (x[0] >= 360.0) x[0] -= 360.0; + x[0] = clamp360(x[0]); // Clamp to 0-360 range //Matrix_print(2,1, x, "6. x"); @@ -223,6 +218,5 @@ Matrix_Copy(2, 2, P, P2); //Matrix_print(2,2, P, "7. P"); - return x[0]; }
--- a/SystemState.h Thu Jun 06 13:40:23 2013 +0000 +++ b/SystemState.h Fri Jun 07 14:45:46 2013 +0000 @@ -55,6 +55,7 @@ unsigned int millis; float current, voltage; int g[3]; + float gyro[3]; int gTemp; int a[3]; int m[3]; @@ -79,7 +80,7 @@ float bearing; float distance; float gbias; - float errAngle; + float errHeading; float steerAngle; } SystemState;
--- a/logging/logging.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/logging/logging.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -13,6 +13,7 @@ s->millis = 0; s->current = s->voltage = 0.0; s->g[0] = s->g[1] = s->g[2] = 0; + s->gyro[0] = s->gyro[1] = s->gyro[2] = 0; s->gTemp = 0; s->a[0] = s->a[1] = s->a[2] = 0; s->m[0] = s->m[1] = s->m[2] = 0; @@ -143,7 +144,7 @@ printFloat(logp, s.voltage, 2); fputc(',',logp); for (int q=0; q < 3; q++) { - printInt(logp, s.g[q]); + printFloat(logp, s.gyro[q], 6); fputc(',',logp); } printInt(logp, s.gTemp); @@ -208,6 +209,8 @@ fputc(',',logp); printFloat(logp, s.steerAngle, 3); fputc(',',logp); + printFloat(logp, s.errHeading, 3); + fputc(',',logp); fputc('\n',logp); t2 = logtimer.read_us();
--- a/main.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/main.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -193,11 +193,12 @@ // totally jacks up everything (noise?) initSteering(); initThrottle(); - // initFlasher(); // Initialize autonomous mode flasher + // initFlasher(); // Initialize autonomous mode flasher // Send data back to the PC pc.baud(115200); fprintf(stdout, "Data Bus 2013\n"); + while (pc.readable()) pc.getc(); // flush buffer on reset display.init(); display.status("Data Bus 2013");
--- a/updater.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/updater.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -27,11 +27,11 @@ //#define LOG_SKIP 1 // 50ms, 20hz, log entry entered into fifo // The following is for main loop at 10ms = 100hz -#define HDG_LAG 100 -#define CTRL_SKIP 5 // 50ms (20hz), control update -#define MAG_SKIP 2 // 20ms (50hz), magnetometer update -#define LOG_SKIP 2 // 20ms (50hz), log entry entered into fifo -//#define LOG_SKIP 5 // 50ms (20hz), log entry entered into fifo +#define HDG_LAG 40 // Number of update steps that the GPS report is behind realtime +#define CTRL_SKIP 5 // 50ms (20hz), control update +#define MAG_SKIP 2 // 20ms (50hz), magnetometer update +#define LOG_SKIP 2 // 20ms (50hz), log entry entered into fifo +//#define LOG_SKIP 5 // 50ms (20hz), log entry entered into fifo Ticker sched; // scheduler for interrupt driven routines @@ -82,7 +82,7 @@ float dt; // dt for the KF float estLagHeading = 0; // lagged heading estimate //GeoPosition lagHere; // lagged position estimate; use here as the current position estimate -float errAngle; // error between gyro hdg estimate and gps hdg estimate +float errHeading; // error between gyro hdg estimate and gps hdg estimate float gyroBias=0; // exponentially weighted moving average of gyro error float Ag = (2.0/(1000.0+1.0)); // Gyro bias filter alpha, gyro; # of 10ms steps float Kbias = 0.995; @@ -227,6 +227,8 @@ sensors.Read_Power(); sensors.Read_Encoders(); + // really need to do some filtering on the speed + //nowSpeed = 0.8*nowSpeed + 0.2*sensors.encSpeed; nowSpeed = sensors.encSpeed; sensors.Read_Gyro(); @@ -301,10 +303,7 @@ history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled history[now].gyro = sensors.gyro[_z_]; // current raw gyro data history[now].dt = dt; // current dt, to address jitter - history[now].hdg = history[prev].hdg + dt*(history[now].gyro - gyroBias); // compute current heading from current gyro - clamp(history[now].hdg, 0, 360.0); - //if (history[now].hdg >= 360.0) history[now].hdg -= 360.0; - //if (history[now].hdg < 0) history[now].hdg += 360.0; + history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro - gyroBias) ); // compute current heading from current gyro float r = PI/180 * history[now].hdg; history[now].x = history[prev].x + history[now].dist * sin(r); // update current position history[now].y = history[prev].y + history[now].dist * cos(r); @@ -365,27 +364,21 @@ // Low pass filter the error correction. Multiplying by 0.01, it takes HDG_LAG updates to correct a // consistent error; that's 0.10s/0.01 = 1 sec. 0.005 is 2 sec, 0.0025 is 4 sec, etc. - errAngle = (estLagHeading - history[lag].hdg); - // low pass filter error angle: - clamp(errAngle, -180.0, 180.0); - //if (errAngle > 180.0) errAngle -= 360.0; - //if (errAngle <= -180.0) errAngle += 360.0; - errAngle *= 0.01; // multiply only after clamping to -180:180 range. + errHeading = clamp180(estLagHeading - history[lag].hdg) * 0.01; // lopass filter error angle - //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, estLagHeading, history[lag].hdg, estLagHeading - history[lag].hdg, errAngle); - float cosA = cos(errAngle * PI / 180.0); - float sinA = sin(errAngle * PI / 180.0); + //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, estLagHeading, history[lag].hdg, estLagHeading - history[lag].hdg, errHeading); + float cosA = cos(errHeading * PI / 180.0); + float sinA = sin(errHeading * PI / 180.0); // Update position & heading from history[lag] through history[now] int i = lag; // TODO 2 parameterize heading lag -- for uBlox it seems to be ~ 600ms, for Venus, about 1000ms for (int j=0; j < HDG_LAG; j++) { //fprintf(stdout, "i=%d n=%d l=%d\n", i, now, lag); // Correct gyro-calculated headings from past to present including history[lag].hdg - history[i].hdg += errAngle; - clamp(history[i].hdg, 0, 360.0); + history[i].hdg = clamp360(history[i].hdg + errHeading); //if (history[i].hdg >= 360.0) history[i].hdg -= 360.0; //if (history[i].hdg < 0) history[i].hdg += 360.0; - // Rotate x, y by errAngle around pivot point; no need to rotate pivot point (j=0) + // Rotate x, y by errHeading around pivot point; no need to rotate pivot point (j=0) if (j > 0) { float dx = history[i].x-history[lag].x; float dy = history[i].y-history[lag].y; @@ -409,11 +402,6 @@ bearing = cartHere.bearingTo(config.cwpt[nextWaypoint]); distance = cartHere.distanceTo(config.cwpt[nextWaypoint]); float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]); - double relativeBrg = bearing - history[now].hdg; - // if correction angle is < -180, express as negative degree - clamp(relativeBrg, -180.0, 180.0); - //if (relativeBrg < -180.0) relativeBrg += 360.0; - //if (relativeBrg > 180.0) relativeBrg -= 360.0; // if within config.waypointDist distance threshold move to next waypoint // TODO 3 figure out how to output feedback on wpt arrival external to this function @@ -446,38 +434,24 @@ ////////////////////////////////////////////////////////////////////////////// // OBSTACLE DETECTION & AVOIDANCE ////////////////////////////////////////////////////////////////////////////// - // TODO: 1 limit steering angle based on object detection ? + // TODO 2 limit steering angle based on object detection ? // or limit relative brg perhaps? - // TODO: 1 add vision obstacle detection and filtering - // TODO: 1 add ranger obstacle detection and filtering/fusion with vision + // TODO 2 add vision obstacle detection and filtering + // TODO 2 add ranger obstacle detection and filtering/fusion with vision ////////////////////////////////////////////////////////////////////////////// // CONTROL UPDATE ////////////////////////////////////////////////////////////////////////////// - // TODO: 1 improve the steering algorithm to take cross-track error into account - if (--control_count == 0) { steerAngle = steerCalc.pathPursuitSA(history[now].hdg, history[now].x, history[now].y, config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y, config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y); - // TODO 3: eliminate pursuit config item - /* - if (config.usePP) { - steerAngle = steerCalc.purePursuitSA(history[now].hdg, - history[now].x, history[now].y, - config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y, - config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y); - } else { - steerAngle = steerCalc.calcSA(relativeBrg, config.minRadius); // use the configured minimum turn radius - } - */ - - // Apply gain factor for near straight line + // TODO 3 figure out a better, continuous way to deal with steering gain if (fabs(steerAngle) < config.steerGainAngle) steerAngle *= config.steerGain; // Curb avoidance @@ -562,6 +536,7 @@ for (int i=0; i < 3; i++) { state[inState].m[i] = sensors.m[i]; state[inState].g[i] = sensors.g[i]; + state[inState].gyro[i] = sensors.gyro[i]; state[inState].a[i] = sensors.a[i]; } state[inState].gTemp = sensors.gTemp; @@ -581,7 +556,7 @@ state[inState].distance = distance; state[inState].nextWaypoint = nextWaypoint; state[inState].gbias = gyroBias; - state[inState].errAngle = biasErrAngle; + state[inState].errHeading = errHeading; //state[inState].leftRanger = sensors.leftRanger; //state[inState].rightRanger = sensors.rightRanger; //state[inState].centerRanger = sensors.centerRanger;
--- a/util.cpp Thu Jun 06 13:40:23 2013 +0000 +++ b/util.cpp Fri Jun 07 14:45:46 2013 +0000 @@ -1,12 +1,24 @@ +#include <math.h> + /** * Clamp a value (angle) between min (non-inclusive) and max (inclusive) * e.g. clamp(v, 0, 360) or clamp(v, -180, 180) */ -float clamp(float v, float min, float max) +float clamp(float v, float min, float max, bool flip) { + float i; + float f; float mod = (max - min); - if (v >= max) float -= mod; - if (v < min) float += mod; + + f = modff((v/mod), &i) * mod; + if (flip) { + if (f > max) f -= mod; + if (f <= min) f += mod; + } else { + if (f < min) f += mod; + if (f >= max) f -= mod; + } + return f; } // convert character to an int
--- a/util.h Thu Jun 06 13:40:23 2013 +0000 +++ b/util.h Fri Jun 07 14:45:46 2013 +0000 @@ -3,17 +3,22 @@ /** Utility routines */ -#define clamp360(x) \ - while ((x) >= 360.0) (x) -= 360.0; \ - while ((x) < 0) (x) += 360.0; -#define clamp180(x) ((x) - floor((x)/360.0) * 360.0 - 180.0); +#define clamp360(x) clamp((x), 0, 360.0, false) +#define clamp180(x) clamp((x), -180.0, 180.0, true) #ifndef M_PI #define M_PI 3.14159265358979323846 #endif -/** Clamp value between min (non-inclusive) and max (inclusive) */ -float clamp(float v, float min, float max); +/** + * Clamp value between min and max. Specify which of the two are inclusive + * @param v is the value to clamp + * @param min is the minimum value, inclusive if flip==false, exclusive otherwise + * @param max is the maximum value, inclusive if flip==true, exclusive otherwise + * @param flip determines whether min or max is inclusive + * @return the clamped value + */ +float clamp(float v, float min, float max, bool flip); /** Convert char to integer */ int ctoi(char c);