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
Diff: Config/Config.cpp
- Revision:
- 9:39c0ff43332b
- Parent:
- 3:42f3821c4e54
- Child:
- 22:dc54ca6e6eec
diff -r aed35fb80b0f -r 39c0ff43332b Config/Config.cpp --- a/Config/Config.cpp Thu Nov 29 16:14:45 2018 +0000 +++ b/Config/Config.cpp Thu Nov 29 16:20:25 2018 +0000 @@ -1,137 +1,185 @@ #define MAXBUF 64 #include "Config.h" -#include "SDHCFileSystem.h" +#include "SDFileSystem.h" #include "GeoPosition.h" #include "globals.h" #include "util.h" -extern Serial pc; +// Identifiers for each of the parameters +#define CURB "curb" +#define WAYPOINT "wpt" +#define GPS "gps" +#define GYRO "gyro" +#define MAGNETOMETER "mag" +#define ACCELEROMETER "accel" +#define DECLINATION "decl" +#define NAVIGATION "nav" +#define STEER "steer" +#define SPEED "speed" +#define VEHICLE "veh" +#define ENCODER "enc" -// TODO: 3: mod waypoints to include speed after waypoint Config::Config(): - loaded(false) + loaded(false), + intercept(0), + waypointDist(0), + brakeDist(0), + wptCount(0), + escMin(1300), + escZero(1300), + escMax(1300), + topSpeed(0), + turnSpeed(0), + startSpeed(0), + minRadius(0), + speedKp(0), + speedKi(0), + speedKd(0), + steerZero(0), + steerScale(0), + curbThreshold(0), + curbGain(0), + gyroScale(0), + // Data Bus Original Settings + wheelbase(0.280), + track(0.290), + tireCirc(0.321537), + encStripes(32) { - // not much to do here, yet. + for (int i=0; i < MAX_WPT; i++) { + wptTopSpeedAdj[i] = 0; + wptTurnSpeedAdj[i] = 0; + } + for (int i=0; i < 3; i++) { + magOffset[i] = 0; + magScale[i] = 0; + } } // load configuration from filesystem -bool Config::load() +bool Config::load(const char *filename) { - LocalFileSystem local("etc"); // Create the local filesystem under the name "local" FILE *fp; char buf[MAXBUF]; // buffer to read in data char tmp[MAXBUF]; // temp buffer char *p; double lat, lon; + float wTopSpeed, wTurnSpeed; bool declFound = false; bool confStatus = false; - - // Just to be safe let's wait - //wait(2.0); pc.printf("opening config file...\n"); - fp = fopen("/etc/config.txt", "r"); + fp = fopen(filename, "r"); if (fp == 0) { - pc.printf("Could not open config.txt\n"); + pc.puts("Could not open "); + pc.puts(filename); + pc.puts(" \n"); } else { wptCount = 0; - declination = 0.0; while (!feof(fp)) { fgets(buf, MAXBUF-1, fp); - p = split(tmp, buf, MAXBUF, ','); // split off the first field - switch (tmp[0]) { - case 'B' : - p = split(tmp, p, MAXBUF, ','); // threshold distance for curb avoid - curbThreshold = cvstof(tmp); - p = split(tmp, p, MAXBUF, ','); // steering gain for curb avoid - curbGain = cvstof(tmp); - break; - case 'W' : // Waypoint - p = split(tmp, p, MAXBUF, ','); // split off the latitude to tmp - lat = cvstof(tmp); - p = split(tmp, p, MAXBUF, ','); // split off the longitude to tmp - lon = cvstof(tmp); - if (wptCount < MAXWPT) { - wpt[wptCount].set(lat, lon); - wptCount++; - } - break; - case 'G' : // GPS - p = split(tmp, p, MAXBUF, ','); - gpsBaud = atoi(tmp); // baud rate for gps - p = split(tmp, p, MAXBUF, ','); - gpsType = atoi(tmp); - break; - case 'Y' : // Gyro Bias - p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp - gyroBias = (float) cvstof(tmp); - break; - case 'D' : // Compass Declination - p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp - declination = (float) cvstof(tmp); - declFound = true; - break; - case 'N' : // Navigation and Turning - p = split(tmp, p, MAXBUF, ','); - interceptDist = (float) cvstof(tmp); // intercept distance for steering algorithm - p = split(tmp, p, MAXBUF, ','); - waypointDist = (float) cvstof(tmp); // distance before waypoint switch - p = split(tmp, p, MAXBUF, ','); - brakeDist = (float) cvstof(tmp); // distance at which braking starts + p = split(tmp, buf, MAXBUF, ','); // split off the first field + + if (!strcmp(tmp, CURB)) { + + p = split(tmp, p, MAXBUF, ','); // threshold distance for curb avoid + curbThreshold = cvstof(tmp); + p = split(tmp, p, MAXBUF, ','); // steering gain for curb avoid + curbGain = cvstof(tmp); + + } else if (!strcmp(tmp, WAYPOINT)) { + + p = split(tmp, p, MAXBUF, ','); // split off the latitude to tmp + lat = cvstof(tmp); + p = split(tmp, p, MAXBUF, ','); // split off the longitude to tmp + lon = cvstof(tmp); + p = split(tmp, p, MAXBUF, ','); // split off the waypoint top speed + wTopSpeed = cvstof(tmp); + p = split(tmp, p, MAXBUF, ','); // split off the waypoint turn speed + wTurnSpeed = cvstof(tmp); + if (wptCount < MAXWPT) { + wpt[wptCount].set(lat, lon); // set position + wptTopSpeedAdj[wptCount] = wTopSpeed; // set top speed adjust + wptTurnSpeedAdj[wptCount] = wTurnSpeed; // set turn speed adjust + wptCount++; + } + + } else if (!strcmp(tmp, NAVIGATION)) { + + p = split(tmp, p, MAXBUF, ','); + intercept = (float) cvstof(tmp); // intercept distance for steering algorithm + p = split(tmp, p, MAXBUF, ','); + waypointDist = (float) cvstof(tmp); // distance before waypoint switch + p = split(tmp, p, MAXBUF, ','); + brakeDist = (float) cvstof(tmp); // distance at which braking starts + p = split(tmp, p, MAXBUF, ','); + minRadius = (float) cvstof(tmp); // minimum turning radius + + } else if (!strcmp(tmp, STEER)) { + + p = split(tmp, p, MAXBUF, ','); + steerZero = cvstof(tmp); // servo center setting + p = split(tmp, p, MAXBUF, ','); + steerScale = cvstof(tmp); // steering angle conversion factor + + } else if (!strcmp(tmp, SPEED)) { + + p = split(tmp, p, MAXBUF, ','); + escMin = cvstof(tmp); // minimum esc (brake) setting + p = split(tmp, p, MAXBUF, ','); + escZero = cvstof(tmp); // esc center/zero setting + p = split(tmp, p, MAXBUF, ','); + escMax = cvstof(tmp); // maximum esc setting + p = split(tmp, p, MAXBUF, ','); + topSpeed = cvstof(tmp); // desired top speed + p = split(tmp, p, MAXBUF, ','); + turnSpeed = cvstof(tmp); // speed to use when turning + p = split(tmp, p, MAXBUF, ','); + startSpeed = cvstof(tmp); // speed to use at start + p = split(tmp, p, MAXBUF, ','); + speedKp = cvstof(tmp); // speed PID: proportional gain + p = split(tmp, p, MAXBUF, ','); + speedKi = cvstof(tmp); // speed PID: integral gain + p = split(tmp, p, MAXBUF, ','); + speedKd = cvstof(tmp); // speed PID: derivative gain + + } else if (!strcmp(tmp, VEHICLE)) { + p = split(tmp, p, MAXBUF, ','); + wheelbase = cvstof(tmp); // vehicle wheelbase (front to rear axle) + p = split(tmp, p, MAXBUF, ','); + track = cvstof(tmp); // vehicle track width (left to right) + } else if (!strcmp(tmp, ENCODER)) { + p = split(tmp, p, MAXBUF, ','); + tireCirc = cvstof(tmp); // tire circumference + p = split(tmp, p, MAXBUF, ','); + encStripes = atoi(tmp); // ticks per revolution + } else if (!strcmp(tmp, GYRO)) { + p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp + gyroScale = cvstof(tmp); // gyro scaling factor to deg/sec + } else if (!strcmp(tmp, GPS)) { + p = split(tmp, p, MAXBUF, ','); + gpsValidSpeed = cvstof(tmp); + } //if-else + /* else if (!strcmp(tmp, DECLINATION)) { + p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp + declination = (float) cvstof(tmp); + declFound = true; + } else if (!strcmp(tmp, MAGNETOMETER)) { + for (int i=0; i < 3; i++) { p = split(tmp, p, MAXBUF, ','); - minRadius = (float) cvstof(tmp); // minimum turning radius - break; - case 'M' : // Magnetometer - for (int i=0; i < 3; i++) { - p = split(tmp, p, MAXBUF, ','); - magOffset[i] = (float) cvstof(tmp); - pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]); - } - for (int i=0; i < 3; i++) { - p = split(tmp, p, MAXBUF, ','); - magScale[i] = (float) cvstof(tmp); - pc.printf("magScale[%d]: %.2f\n", i, magScale[i]); - } - case 'R' : // Steering configuration - p = split(tmp, p, MAXBUF, ','); - steerZero = cvstof(tmp); // servo center setting - p = split(tmp, p, MAXBUF, ','); - steerGain = cvstof(tmp); // steering angle multiplier - p = split(tmp, p, MAXBUF, ','); - steerGainAngle = cvstof(tmp); // angle below which steering gain takes effect - break; - case 'S' : // Throttle configuration + magOffset[i] = (float) cvstof(tmp); + pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]); + } + for (int i=0; i < 3; i++) { p = split(tmp, p, MAXBUF, ','); - escMin = atoi(tmp); // minimum esc (brake) setting - p = split(tmp, p, MAXBUF, ','); - escZero = atoi(tmp); // esc center/zero setting - p = split(tmp, p, MAXBUF, ','); - escMax = atoi(tmp); // maximum esc setting - p = split(tmp, p, MAXBUF, ','); - topSpeed = cvstof(tmp); // desired top speed - p = split(tmp, p, MAXBUF, ','); - turnSpeed = cvstof(tmp); // speed to use within brake distance of waypoint - p = split(tmp, p, MAXBUF, ','); - startSpeed = cvstof(tmp); // speed to use at start - p = split(tmp, p, MAXBUF, ','); - speedKp = cvstof(tmp); // speed PID: proportional gain - p = split(tmp, p, MAXBUF, ','); - speedKi = cvstof(tmp); // speed PID: integral gain - p = split(tmp, p, MAXBUF, ','); - speedKd = cvstof(tmp); // speed PID: derivative gain - - break; - case 'E' : - p = split(tmp, p, MAXBUF, ','); - compassGain = (float) cvstof(tmp); // not used (DCM) - p = split(tmp, p, MAXBUF, ','); - yawGain = (float) cvstof(tmp); // not used (DCM) - default : - break; - } // switch + magScale[i] = (float) cvstof(tmp); + pc.printf("magScale[%d]: %.2f\n", i, magScale[i]); + } + } + */ } // while // Did we get the values we were looking for? @@ -147,4 +195,4 @@ loaded = confStatus; return confStatus; -} +} \ No newline at end of file