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?
User | Revision | Line number | New 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 | } |