Michael Shimniok / Mbed 2 deprecated DataBus

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@0:a6a169de725f, 2013-05-27 (annotated)

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Child:
1:cb84b477886c
Working version with priorities set and update time display

Who changed what in which revision?

UserRevisionLine numberNew 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 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 105 cteKi = cvstof(tmp); // integral term for cross track
shimniok 0:a6a169de725f 106 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 107 usePP = ( atoi(tmp) == 1 ); // use pure pursuit algorithm
shimniok 0:a6a169de725f 108 break;
shimniok 0:a6a169de725f 109 case 'S' : // Throttle configuration
shimniok 0:a6a169de725f 110 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 111 escMin = atoi(tmp); // minimum esc (brake) setting
shimniok 0:a6a169de725f 112 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 113 escZero = atoi(tmp); // esc center/zero setting
shimniok 0:a6a169de725f 114 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 115 escMax = atoi(tmp); // maximum esc setting
shimniok 0:a6a169de725f 116 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 117 topSpeed = cvstof(tmp); // desired top speed
shimniok 0:a6a169de725f 118 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 119 turnSpeed = cvstof(tmp); // speed to use within brake distance of waypoint
shimniok 0:a6a169de725f 120 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 121 startSpeed = cvstof(tmp); // speed to use at start
shimniok 0:a6a169de725f 122 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 123 speedKp = cvstof(tmp); // speed PID: proportional gain
shimniok 0:a6a169de725f 124 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 125 speedKi = cvstof(tmp); // speed PID: integral gain
shimniok 0:a6a169de725f 126 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 127 speedKd = cvstof(tmp); // speed PID: derivative gain
shimniok 0:a6a169de725f 128
shimniok 0:a6a169de725f 129 break;
shimniok 0:a6a169de725f 130 case 'E' :
shimniok 0:a6a169de725f 131 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 132 compassGain = (float) cvstof(tmp); // not used (DCM)
shimniok 0:a6a169de725f 133 p = split(tmp, p, MAXBUF, ',');
shimniok 0:a6a169de725f 134 yawGain = (float) cvstof(tmp); // not used (DCM)
shimniok 0:a6a169de725f 135 default :
shimniok 0:a6a169de725f 136 break;
shimniok 0:a6a169de725f 137 } // switch
shimniok 0:a6a169de725f 138 } // while
shimniok 0:a6a169de725f 139
shimniok 0:a6a169de725f 140 // Did we get the values we were looking for?
shimniok 0:a6a169de725f 141 if (wptCount > 0 && declFound) {
shimniok 0:a6a169de725f 142 confStatus = true;
shimniok 0:a6a169de725f 143 }
shimniok 0:a6a169de725f 144
shimniok 0:a6a169de725f 145 } // if fp
shimniok 0:a6a169de725f 146
shimniok 0:a6a169de725f 147 if (fp != 0)
shimniok 0:a6a169de725f 148 fclose(fp);
shimniok 0:a6a169de725f 149
shimniok 0:a6a169de725f 150 return confStatus;
shimniok 0:a6a169de725f 151 }