Michael Shimniok / Mbed 2 deprecated AVC_2012

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@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:826c6171fc1b 1 #define MAXBUF 64
shimniok 0:826c6171fc1b 2
shimniok 0:826c6171fc1b 3 #include "Config.h"
shimniok 0:826c6171fc1b 4 #include "SDHCFileSystem.h"
shimniok 0:826c6171fc1b 5 #include "GeoPosition.h"
shimniok 0:826c6171fc1b 6 #include "globals.h"
shimniok 0:826c6171fc1b 7 #include "util.h"
shimniok 0:826c6171fc1b 8
shimniok 0:826c6171fc1b 9 extern Serial pc;
shimniok 0:826c6171fc1b 10
shimniok 0:826c6171fc1b 11 // TODO: 3: mod waypoints to include speed after waypoint
shimniok 0:826c6171fc1b 12
shimniok 0:826c6171fc1b 13 Config::Config()
shimniok 0:826c6171fc1b 14 {
shimniok 0:826c6171fc1b 15 // not much to do here, yet.
shimniok 0:826c6171fc1b 16 }
shimniok 0:826c6171fc1b 17
shimniok 0:826c6171fc1b 18 // load configuration from filesystem
shimniok 0:826c6171fc1b 19 bool Config::load()
shimniok 0:826c6171fc1b 20 {
shimniok 0:826c6171fc1b 21 LocalFileSystem local("etc"); // Create the local filesystem under the name "local"
shimniok 0:826c6171fc1b 22 FILE *fp;
shimniok 0:826c6171fc1b 23 char buf[MAXBUF]; // buffer to read in data
shimniok 0:826c6171fc1b 24 char tmp[MAXBUF]; // temp buffer
shimniok 0:826c6171fc1b 25 char *p;
shimniok 0:826c6171fc1b 26 double lat, lon;
shimniok 0:826c6171fc1b 27 bool declFound = false;
shimniok 0:826c6171fc1b 28 bool confStatus = false;
shimniok 0:826c6171fc1b 29
shimniok 0:826c6171fc1b 30 // Just to be safe let's wait
shimniok 0:826c6171fc1b 31 //wait(2.0);
shimniok 0:826c6171fc1b 32
shimniok 0:826c6171fc1b 33 pc.printf("opening config file...\n");
shimniok 0:826c6171fc1b 34
shimniok 0:826c6171fc1b 35 fp = fopen("/etc/config.txt", "r");
shimniok 0:826c6171fc1b 36 if (fp == 0) {
shimniok 0:826c6171fc1b 37 pc.printf("Could not open config.txt\n");
shimniok 0:826c6171fc1b 38 } else {
shimniok 0:826c6171fc1b 39 wptCount = 0;
shimniok 0:826c6171fc1b 40 declination = 0.0;
shimniok 0:826c6171fc1b 41 while (!feof(fp)) {
shimniok 0:826c6171fc1b 42 fgets(buf, MAXBUF-1, fp);
shimniok 0:826c6171fc1b 43 p = split(tmp, buf, MAXBUF, ','); // split off the first field
shimniok 0:826c6171fc1b 44 switch (tmp[0]) {
shimniok 0:826c6171fc1b 45 case 'B' :
shimniok 0:826c6171fc1b 46 p = split(tmp, p, MAXBUF, ','); // threshold distance for curb avoid
shimniok 0:826c6171fc1b 47 curbThreshold = cvstof(tmp);
shimniok 0:826c6171fc1b 48 p = split(tmp, p, MAXBUF, ','); // steering gain for curb avoid
shimniok 0:826c6171fc1b 49 curbGain = cvstof(tmp);
shimniok 0:826c6171fc1b 50 break;
shimniok 0:826c6171fc1b 51 case 'W' : // Waypoint
shimniok 0:826c6171fc1b 52 p = split(tmp, p, MAXBUF, ','); // split off the latitude to tmp
shimniok 0:826c6171fc1b 53 lat = cvstof(tmp);
shimniok 0:826c6171fc1b 54 p = split(tmp, p, MAXBUF, ','); // split off the longitude to tmp
shimniok 0:826c6171fc1b 55 lon = cvstof(tmp);
shimniok 0:826c6171fc1b 56 if (wptCount < MAXWPT) {
shimniok 0:826c6171fc1b 57 wpt[wptCount].set(lat, lon);
shimniok 0:826c6171fc1b 58 wptCount++;
shimniok 0:826c6171fc1b 59 }
shimniok 0:826c6171fc1b 60 break;
shimniok 0:826c6171fc1b 61 case 'G' : // GPS
shimniok 0:826c6171fc1b 62 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 63 gpsBaud = atoi(tmp); // baud rate for gps
shimniok 0:826c6171fc1b 64 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 65 gpsType = atoi(tmp);
shimniok 0:826c6171fc1b 66 break;
shimniok 0:826c6171fc1b 67 case 'Y' : // Gyro Bias
shimniok 0:826c6171fc1b 68 p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
shimniok 0:826c6171fc1b 69 gyroBias = (float) cvstof(tmp);
shimniok 0:826c6171fc1b 70 break;
shimniok 0:826c6171fc1b 71 case 'D' : // Compass Declination
shimniok 0:826c6171fc1b 72 p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
shimniok 0:826c6171fc1b 73 declination = (float) cvstof(tmp);
shimniok 0:826c6171fc1b 74 declFound = true;
shimniok 0:826c6171fc1b 75 break;
shimniok 0:826c6171fc1b 76 case 'N' : // Navigation and Turning
shimniok 0:826c6171fc1b 77 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 78 interceptDist = (float) cvstof(tmp); // intercept distance for steering algorithm
shimniok 0:826c6171fc1b 79 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 80 waypointDist = (float) cvstof(tmp); // distance before waypoint switch
shimniok 0:826c6171fc1b 81 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 82 brakeDist = (float) cvstof(tmp); // distance at which braking starts
shimniok 0:826c6171fc1b 83 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 84 minRadius = (float) cvstof(tmp); // minimum turning radius
shimniok 0:826c6171fc1b 85 break;
shimniok 0:826c6171fc1b 86 case 'M' : // Magnetometer
shimniok 0:826c6171fc1b 87 for (int i=0; i < 3; i++) {
shimniok 0:826c6171fc1b 88 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 89 magOffset[i] = (float) cvstof(tmp);
shimniok 0:826c6171fc1b 90 pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]);
shimniok 0:826c6171fc1b 91 }
shimniok 0:826c6171fc1b 92 for (int i=0; i < 3; i++) {
shimniok 0:826c6171fc1b 93 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 94 magScale[i] = (float) cvstof(tmp);
shimniok 0:826c6171fc1b 95 pc.printf("magScale[%d]: %.2f\n", i, magScale[i]);
shimniok 0:826c6171fc1b 96 }
shimniok 0:826c6171fc1b 97 case 'R' : // Steering configuration
shimniok 0:826c6171fc1b 98 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 99 steerZero = cvstof(tmp); // servo center setting
shimniok 0:826c6171fc1b 100 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 101 steerGain = cvstof(tmp); // steering angle multiplier
shimniok 0:826c6171fc1b 102 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 103 steerGainAngle = cvstof(tmp); // angle below which steering gain takes effect
shimniok 0:826c6171fc1b 104 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 105 cteKi = cvstof(tmp); // integral term for cross track
shimniok 0:826c6171fc1b 106 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 107 usePP = ( atoi(tmp) == 1 ); // use pure pursuit algorithm
shimniok 0:826c6171fc1b 108 break;
shimniok 0:826c6171fc1b 109 case 'S' : // Throttle configuration
shimniok 0:826c6171fc1b 110 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 111 escMin = atoi(tmp); // minimum esc (brake) setting
shimniok 0:826c6171fc1b 112 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 113 escZero = atoi(tmp); // esc center/zero setting
shimniok 0:826c6171fc1b 114 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 115 escMax = atoi(tmp); // maximum esc setting
shimniok 0:826c6171fc1b 116 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 117 topSpeed = cvstof(tmp); // desired top speed
shimniok 0:826c6171fc1b 118 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 119 turnSpeed = cvstof(tmp); // speed to use within brake distance of waypoint
shimniok 0:826c6171fc1b 120 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 121 startSpeed = cvstof(tmp); // speed to use at start
shimniok 0:826c6171fc1b 122 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 123 speedKp = cvstof(tmp); // speed PID: proportional gain
shimniok 0:826c6171fc1b 124 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 125 speedKi = cvstof(tmp); // speed PID: integral gain
shimniok 0:826c6171fc1b 126 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 127 speedKd = cvstof(tmp); // speed PID: derivative gain
shimniok 0:826c6171fc1b 128
shimniok 0:826c6171fc1b 129 break;
shimniok 0:826c6171fc1b 130 case 'E' :
shimniok 0:826c6171fc1b 131 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 132 compassGain = (float) cvstof(tmp); // not used (DCM)
shimniok 0:826c6171fc1b 133 p = split(tmp, p, MAXBUF, ',');
shimniok 0:826c6171fc1b 134 yawGain = (float) cvstof(tmp); // not used (DCM)
shimniok 0:826c6171fc1b 135 default :
shimniok 0:826c6171fc1b 136 break;
shimniok 0:826c6171fc1b 137 } // switch
shimniok 0:826c6171fc1b 138 } // while
shimniok 0:826c6171fc1b 139
shimniok 0:826c6171fc1b 140 // Did we get the values we were looking for?
shimniok 0:826c6171fc1b 141 if (wptCount > 0 && declFound) {
shimniok 0:826c6171fc1b 142 confStatus = true;
shimniok 0:826c6171fc1b 143 }
shimniok 0:826c6171fc1b 144
shimniok 0:826c6171fc1b 145 } // if fp
shimniok 0:826c6171fc1b 146
shimniok 0:826c6171fc1b 147 if (fp != 0)
shimniok 0:826c6171fc1b 148 fclose(fp);
shimniok 0:826c6171fc1b 149
shimniok 0:826c6171fc1b 150 return confStatus;
shimniok 0:826c6171fc1b 151 }