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