Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Config/Config.cpp
- Committer:
- shimniok
- Date:
- 2012-06-20
- Revision:
- 0:826c6171fc1b
File content as of revision 0:826c6171fc1b:
#define MAXBUF 64 #include "Config.h" #include "SDHCFileSystem.h" #include "GeoPosition.h" #include "globals.h" #include "util.h" extern Serial pc; // TODO: 3: mod waypoints to include speed after waypoint Config::Config() { // not much to do here, yet. } // load configuration from filesystem bool Config::load() { 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; 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"); if (fp == 0) { pc.printf("Could not open config.txt\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, 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 p = split(tmp, p, MAXBUF, ','); cteKi = cvstof(tmp); // integral term for cross track p = split(tmp, p, MAXBUF, ','); usePP = ( atoi(tmp) == 1 ); // use pure pursuit algorithm break; case 'S' : // Throttle configuration 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 } // while // Did we get the values we were looking for? if (wptCount > 0 && declFound) { confStatus = true; } } // if fp if (fp != 0) fclose(fp); return confStatus; }