Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Config.cpp Source File

Config.cpp

00001 #define MAXBUF 64
00002 
00003 #include "Config.h"
00004 #include "SDHCFileSystem.h"
00005 #include "GeoPosition.h"
00006 #include "globals.h"
00007 #include "util.h"
00008 
00009 extern Serial pc;
00010 
00011 // TODO: 3: mod waypoints to include speed after waypoint
00012 
00013 Config::Config()
00014 {
00015     // not much to do here, yet.
00016 }
00017 
00018 // load configuration from filesystem
00019 bool Config::load()
00020 {
00021     LocalFileSystem local("etc");         // Create the local filesystem under the name "local"
00022     FILE *fp;
00023     char buf[MAXBUF];   // buffer to read in data
00024     char tmp[MAXBUF];   // temp buffer
00025     char *p;
00026     double lat, lon;
00027     bool declFound = false;
00028     bool confStatus = false;
00029     
00030     // Just to be safe let's wait
00031     //wait(2.0);
00032 
00033     pc.printf("opening config file...\n");
00034     
00035     fp = fopen("/etc/config.txt", "r");
00036     if (fp == 0) {
00037         pc.printf("Could not open config.txt\n");
00038     } else {
00039         wptCount = 0;
00040         declination = 0.0;
00041         while (!feof(fp)) {
00042             fgets(buf, MAXBUF-1, fp);
00043             p = split(tmp, buf, MAXBUF, ',');           // split off the first field
00044             switch (tmp[0]) {
00045                 case 'B' :
00046                     p = split(tmp, p, MAXBUF, ',');     // threshold distance for curb avoid
00047                     curbThreshold = cvstof(tmp);
00048                     p = split(tmp, p, MAXBUF, ',');     // steering gain for curb avoid
00049                     curbGain = cvstof(tmp);
00050                     break;
00051                 case 'W' :                              // Waypoint
00052                     p = split(tmp, p, MAXBUF, ',');     // split off the latitude to tmp
00053                     lat = cvstof(tmp);
00054                     p = split(tmp, p, MAXBUF, ',');     // split off the longitude to tmp
00055                     lon = cvstof(tmp);
00056                     if (wptCount < MAXWPT) {
00057                         wpt[wptCount].set(lat, lon);
00058                         wptCount++;
00059                     }
00060                     break;
00061                 case 'G' :                              // GPS
00062                     p = split(tmp, p, MAXBUF, ',');
00063                     gpsBaud = atoi(tmp);                // baud rate for gps
00064                     p = split(tmp, p, MAXBUF, ',');
00065                     gpsType = atoi(tmp);
00066                     break;
00067                 case 'Y' :                              // Gyro Bias
00068                     p = split(tmp, p, MAXBUF, ',');     // split off the declination to tmp
00069                     gyroBias = (float) cvstof(tmp);
00070                     break;
00071                 case 'D' :                              // Compass Declination
00072                     p = split(tmp, p, MAXBUF, ',');     // split off the declination to tmp
00073                     declination = (float) cvstof(tmp);
00074                     declFound = true;
00075                     break;
00076                 case 'N' :                                  // Navigation and Turning    
00077                     p = split(tmp, p, MAXBUF, ',');
00078                     interceptDist = (float) cvstof(tmp);    // intercept distance for steering algorithm
00079                     p = split(tmp, p, MAXBUF, ',');
00080                     waypointDist = (float) cvstof(tmp);     // distance before waypoint switch
00081                     p = split(tmp, p, MAXBUF, ',');
00082                     brakeDist = (float) cvstof(tmp);        // distance at which braking starts
00083                     p = split(tmp, p, MAXBUF, ',');
00084                     minRadius = (float) cvstof(tmp);        // minimum turning radius
00085                     break;
00086                 case 'M' :                              // Magnetometer
00087                     for (int i=0; i < 3; i++) {
00088                         p = split(tmp, p, MAXBUF, ',');
00089                         magOffset[i] = (float) cvstof(tmp);
00090                         pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]);
00091                     }
00092                     for (int i=0; i < 3; i++) {
00093                         p = split(tmp, p, MAXBUF, ',');
00094                         magScale[i] = (float) cvstof(tmp);
00095                         pc.printf("magScale[%d]: %.2f\n", i, magScale[i]);
00096                     }                    
00097                 case 'R' : // Steering configuration
00098                     p = split(tmp, p, MAXBUF, ',');
00099                     steerZero = cvstof(tmp);            // servo center setting
00100                     p = split(tmp, p, MAXBUF, ',');
00101                     steerGain = cvstof(tmp);            // steering angle multiplier
00102                     p = split(tmp, p, MAXBUF, ',');
00103                     steerGainAngle = cvstof(tmp);       // angle below which steering gain takes effect
00104                     p = split(tmp, p, MAXBUF, ',');
00105                     cteKi = cvstof(tmp);                // integral term for cross track
00106                     p = split(tmp, p, MAXBUF, ',');
00107                     usePP = ( atoi(tmp) == 1 );         // use pure pursuit algorithm
00108                     break;
00109                 case 'S' :                              // Throttle configuration
00110                     p = split(tmp, p, MAXBUF, ',');
00111                     escMin = atoi(tmp);                 // minimum esc (brake) setting
00112                     p = split(tmp, p, MAXBUF, ',');     
00113                     escZero = atoi(tmp);                // esc center/zero setting
00114                     p = split(tmp, p, MAXBUF, ',');     
00115                     escMax = atoi(tmp);                 // maximum esc setting
00116                     p = split(tmp, p, MAXBUF, ',');   
00117                     topSpeed = cvstof(tmp);             // desired top speed
00118                     p = split(tmp, p, MAXBUF, ',');   
00119                     turnSpeed = cvstof(tmp);            // speed to use within brake distance of waypoint
00120                     p = split(tmp, p, MAXBUF, ',');
00121                     startSpeed = cvstof(tmp);            // speed to use at start
00122                     p = split(tmp, p, MAXBUF, ',');
00123                     speedKp = cvstof(tmp);              // speed PID: proportional gain
00124                     p = split(tmp, p, MAXBUF, ',');     
00125                     speedKi = cvstof(tmp);              // speed PID: integral gain
00126                     p = split(tmp, p, MAXBUF, ',');     
00127                     speedKd = cvstof(tmp);              // speed PID: derivative gain
00128     
00129                     break;
00130                 case 'E' :
00131                     p = split(tmp, p, MAXBUF, ',');     
00132                     compassGain = (float) cvstof(tmp);  // not used (DCM)
00133                     p = split(tmp, p, MAXBUF, ',');    
00134                     yawGain = (float) cvstof(tmp);      // not used (DCM)
00135                 default :
00136                     break;
00137             } // switch
00138         } // while
00139 
00140         // Did we get the values we were looking for?
00141         if (wptCount > 0 && declFound) {
00142             confStatus = true;
00143         }
00144         
00145     } // if fp
00146     
00147     if (fp != 0)
00148         fclose(fp);
00149 
00150     return confStatus;
00151 }