Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
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 }
Generated on Tue Jul 12 2022 14:09:25 by 1.7.2