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@3:42f3821c4e54, 2013-06-07 (annotated)

Committer:
shimniok
Date:
Fri Jun 07 14:45:46 2013 +0000
Revision:
3:42f3821c4e54
Parent:
1:cb84b477886c
Child:
9:39c0ff43332b
Working version 6/6/2013. Heading estimation may not be quite perfect yet but seems the major estimation bugs are fixed now.

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