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@9:39c0ff43332b, 2018-11-29 (annotated)

Committer:
shimniok
Date:
Thu Nov 29 16:20:25 2018 +0000
Revision:
9:39c0ff43332b
Parent:
3:42f3821c4e54
Child:
22:dc54ca6e6eec
Deleted Actuators, modified config to latest

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 9:39c0ff43332b 4 #include "SDFileSystem.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 9:39c0ff43332b 9 // Identifiers for each of the parameters
shimniok 9:39c0ff43332b 10 #define CURB "curb"
shimniok 9:39c0ff43332b 11 #define WAYPOINT "wpt"
shimniok 9:39c0ff43332b 12 #define GPS "gps"
shimniok 9:39c0ff43332b 13 #define GYRO "gyro"
shimniok 9:39c0ff43332b 14 #define MAGNETOMETER "mag"
shimniok 9:39c0ff43332b 15 #define ACCELEROMETER "accel"
shimniok 9:39c0ff43332b 16 #define DECLINATION "decl"
shimniok 9:39c0ff43332b 17 #define NAVIGATION "nav"
shimniok 9:39c0ff43332b 18 #define STEER "steer"
shimniok 9:39c0ff43332b 19 #define SPEED "speed"
shimniok 9:39c0ff43332b 20 #define VEHICLE "veh"
shimniok 9:39c0ff43332b 21 #define ENCODER "enc"
shimniok 0:a6a169de725f 22
shimniok 0:a6a169de725f 23
shimniok 3:42f3821c4e54 24 Config::Config():
shimniok 9:39c0ff43332b 25 loaded(false),
shimniok 9:39c0ff43332b 26 intercept(0),
shimniok 9:39c0ff43332b 27 waypointDist(0),
shimniok 9:39c0ff43332b 28 brakeDist(0),
shimniok 9:39c0ff43332b 29 wptCount(0),
shimniok 9:39c0ff43332b 30 escMin(1300),
shimniok 9:39c0ff43332b 31 escZero(1300),
shimniok 9:39c0ff43332b 32 escMax(1300),
shimniok 9:39c0ff43332b 33 topSpeed(0),
shimniok 9:39c0ff43332b 34 turnSpeed(0),
shimniok 9:39c0ff43332b 35 startSpeed(0),
shimniok 9:39c0ff43332b 36 minRadius(0),
shimniok 9:39c0ff43332b 37 speedKp(0),
shimniok 9:39c0ff43332b 38 speedKi(0),
shimniok 9:39c0ff43332b 39 speedKd(0),
shimniok 9:39c0ff43332b 40 steerZero(0),
shimniok 9:39c0ff43332b 41 steerScale(0),
shimniok 9:39c0ff43332b 42 curbThreshold(0),
shimniok 9:39c0ff43332b 43 curbGain(0),
shimniok 9:39c0ff43332b 44 gyroScale(0),
shimniok 9:39c0ff43332b 45 // Data Bus Original Settings
shimniok 9:39c0ff43332b 46 wheelbase(0.280),
shimniok 9:39c0ff43332b 47 track(0.290),
shimniok 9:39c0ff43332b 48 tireCirc(0.321537),
shimniok 9:39c0ff43332b 49 encStripes(32)
shimniok 0:a6a169de725f 50 {
shimniok 9:39c0ff43332b 51 for (int i=0; i < MAX_WPT; i++) {
shimniok 9:39c0ff43332b 52 wptTopSpeedAdj[i] = 0;
shimniok 9:39c0ff43332b 53 wptTurnSpeedAdj[i] = 0;
shimniok 9:39c0ff43332b 54 }
shimniok 9:39c0ff43332b 55 for (int i=0; i < 3; i++) {
shimniok 9:39c0ff43332b 56 magOffset[i] = 0;
shimniok 9:39c0ff43332b 57 magScale[i] = 0;
shimniok 9:39c0ff43332b 58 }
shimniok 0:a6a169de725f 59 }
shimniok 0:a6a169de725f 60
shimniok 0:a6a169de725f 61 // load configuration from filesystem
shimniok 9:39c0ff43332b 62 bool Config::load(const char *filename)
shimniok 0:a6a169de725f 63 {
shimniok 0:a6a169de725f 64 FILE *fp;
shimniok 0:a6a169de725f 65 char buf[MAXBUF]; // buffer to read in data
shimniok 0:a6a169de725f 66 char tmp[MAXBUF]; // temp buffer
shimniok 0:a6a169de725f 67 char *p;
shimniok 0:a6a169de725f 68 double lat, lon;
shimniok 9:39c0ff43332b 69 float wTopSpeed, wTurnSpeed;
shimniok 0:a6a169de725f 70 bool declFound = false;
shimniok 0:a6a169de725f 71 bool confStatus = false;
shimniok 0:a6a169de725f 72
shimniok 0:a6a169de725f 73 pc.printf("opening config file...\n");
shimniok 0:a6a169de725f 74
shimniok 9:39c0ff43332b 75 fp = fopen(filename, "r");
shimniok 0:a6a169de725f 76 if (fp == 0) {
shimniok 9:39c0ff43332b 77 pc.puts("Could not open ");
shimniok 9:39c0ff43332b 78 pc.puts(filename);
shimniok 9:39c0ff43332b 79 pc.puts(" \n");
shimniok 0:a6a169de725f 80 } else {
shimniok 0:a6a169de725f 81 wptCount = 0;
shimniok 0:a6a169de725f 82 while (!feof(fp)) {
shimniok 0:a6a169de725f 83 fgets(buf, MAXBUF-1, fp);
shimniok 9:39c0ff43332b 84 p = split(tmp, buf, MAXBUF, ','); // split off the first field
shimniok 9:39c0ff43332b 85
shimniok 9:39c0ff43332b 86 if (!strcmp(tmp, CURB)) {
shimniok 9:39c0ff43332b 87
shimniok 9:39c0ff43332b 88 p = split(tmp, p, MAXBUF, ','); // threshold distance for curb avoid
shimniok 9:39c0ff43332b 89 curbThreshold = cvstof(tmp);
shimniok 9:39c0ff43332b 90 p = split(tmp, p, MAXBUF, ','); // steering gain for curb avoid
shimniok 9:39c0ff43332b 91 curbGain = cvstof(tmp);
shimniok 9:39c0ff43332b 92
shimniok 9:39c0ff43332b 93 } else if (!strcmp(tmp, WAYPOINT)) {
shimniok 9:39c0ff43332b 94
shimniok 9:39c0ff43332b 95 p = split(tmp, p, MAXBUF, ','); // split off the latitude to tmp
shimniok 9:39c0ff43332b 96 lat = cvstof(tmp);
shimniok 9:39c0ff43332b 97 p = split(tmp, p, MAXBUF, ','); // split off the longitude to tmp
shimniok 9:39c0ff43332b 98 lon = cvstof(tmp);
shimniok 9:39c0ff43332b 99 p = split(tmp, p, MAXBUF, ','); // split off the waypoint top speed
shimniok 9:39c0ff43332b 100 wTopSpeed = cvstof(tmp);
shimniok 9:39c0ff43332b 101 p = split(tmp, p, MAXBUF, ','); // split off the waypoint turn speed
shimniok 9:39c0ff43332b 102 wTurnSpeed = cvstof(tmp);
shimniok 9:39c0ff43332b 103 if (wptCount < MAXWPT) {
shimniok 9:39c0ff43332b 104 wpt[wptCount].set(lat, lon); // set position
shimniok 9:39c0ff43332b 105 wptTopSpeedAdj[wptCount] = wTopSpeed; // set top speed adjust
shimniok 9:39c0ff43332b 106 wptTurnSpeedAdj[wptCount] = wTurnSpeed; // set turn speed adjust
shimniok 9:39c0ff43332b 107 wptCount++;
shimniok 9:39c0ff43332b 108 }
shimniok 9:39c0ff43332b 109
shimniok 9:39c0ff43332b 110 } else if (!strcmp(tmp, NAVIGATION)) {
shimniok 9:39c0ff43332b 111
shimniok 9:39c0ff43332b 112 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 113 intercept = (float) cvstof(tmp); // intercept distance for steering algorithm
shimniok 9:39c0ff43332b 114 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 115 waypointDist = (float) cvstof(tmp); // distance before waypoint switch
shimniok 9:39c0ff43332b 116 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 117 brakeDist = (float) cvstof(tmp); // distance at which braking starts
shimniok 9:39c0ff43332b 118 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 119 minRadius = (float) cvstof(tmp); // minimum turning radius
shimniok 9:39c0ff43332b 120
shimniok 9:39c0ff43332b 121 } else if (!strcmp(tmp, STEER)) {
shimniok 9:39c0ff43332b 122
shimniok 9:39c0ff43332b 123 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 124 steerZero = cvstof(tmp); // servo center setting
shimniok 9:39c0ff43332b 125 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 126 steerScale = cvstof(tmp); // steering angle conversion factor
shimniok 9:39c0ff43332b 127
shimniok 9:39c0ff43332b 128 } else if (!strcmp(tmp, SPEED)) {
shimniok 9:39c0ff43332b 129
shimniok 9:39c0ff43332b 130 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 131 escMin = cvstof(tmp); // minimum esc (brake) setting
shimniok 9:39c0ff43332b 132 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 133 escZero = cvstof(tmp); // esc center/zero setting
shimniok 9:39c0ff43332b 134 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 135 escMax = cvstof(tmp); // maximum esc setting
shimniok 9:39c0ff43332b 136 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 137 topSpeed = cvstof(tmp); // desired top speed
shimniok 9:39c0ff43332b 138 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 139 turnSpeed = cvstof(tmp); // speed to use when turning
shimniok 9:39c0ff43332b 140 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 141 startSpeed = cvstof(tmp); // speed to use at start
shimniok 9:39c0ff43332b 142 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 143 speedKp = cvstof(tmp); // speed PID: proportional gain
shimniok 9:39c0ff43332b 144 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 145 speedKi = cvstof(tmp); // speed PID: integral gain
shimniok 9:39c0ff43332b 146 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 147 speedKd = cvstof(tmp); // speed PID: derivative gain
shimniok 9:39c0ff43332b 148
shimniok 9:39c0ff43332b 149 } else if (!strcmp(tmp, VEHICLE)) {
shimniok 9:39c0ff43332b 150 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 151 wheelbase = cvstof(tmp); // vehicle wheelbase (front to rear axle)
shimniok 9:39c0ff43332b 152 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 153 track = cvstof(tmp); // vehicle track width (left to right)
shimniok 9:39c0ff43332b 154 } else if (!strcmp(tmp, ENCODER)) {
shimniok 9:39c0ff43332b 155 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 156 tireCirc = cvstof(tmp); // tire circumference
shimniok 9:39c0ff43332b 157 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 158 encStripes = atoi(tmp); // ticks per revolution
shimniok 9:39c0ff43332b 159 } else if (!strcmp(tmp, GYRO)) {
shimniok 9:39c0ff43332b 160 p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
shimniok 9:39c0ff43332b 161 gyroScale = cvstof(tmp); // gyro scaling factor to deg/sec
shimniok 9:39c0ff43332b 162 } else if (!strcmp(tmp, GPS)) {
shimniok 9:39c0ff43332b 163 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 164 gpsValidSpeed = cvstof(tmp);
shimniok 9:39c0ff43332b 165 } //if-else
shimniok 9:39c0ff43332b 166 /* else if (!strcmp(tmp, DECLINATION)) {
shimniok 9:39c0ff43332b 167 p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
shimniok 9:39c0ff43332b 168 declination = (float) cvstof(tmp);
shimniok 9:39c0ff43332b 169 declFound = true;
shimniok 9:39c0ff43332b 170 } else if (!strcmp(tmp, MAGNETOMETER)) {
shimniok 9:39c0ff43332b 171 for (int i=0; i < 3; i++) {
shimniok 0:a6a169de725f 172 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 173 magOffset[i] = (float) cvstof(tmp);
shimniok 9:39c0ff43332b 174 pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]);
shimniok 9:39c0ff43332b 175 }
shimniok 9:39c0ff43332b 176 for (int i=0; i < 3; i++) {
shimniok 0:a6a169de725f 177 p = split(tmp, p, MAXBUF, ',');
shimniok 9:39c0ff43332b 178 magScale[i] = (float) cvstof(tmp);
shimniok 9:39c0ff43332b 179 pc.printf("magScale[%d]: %.2f\n", i, magScale[i]);
shimniok 9:39c0ff43332b 180 }
shimniok 9:39c0ff43332b 181 }
shimniok 9:39c0ff43332b 182 */
shimniok 0:a6a169de725f 183 } // while
shimniok 0:a6a169de725f 184
shimniok 0:a6a169de725f 185 // Did we get the values we were looking for?
shimniok 0:a6a169de725f 186 if (wptCount > 0 && declFound) {
shimniok 0:a6a169de725f 187 confStatus = true;
shimniok 0:a6a169de725f 188 }
shimniok 0:a6a169de725f 189
shimniok 0:a6a169de725f 190 } // if fp
shimniok 0:a6a169de725f 191
shimniok 0:a6a169de725f 192 if (fp != 0)
shimniok 0:a6a169de725f 193 fclose(fp);
shimniok 0:a6a169de725f 194
shimniok 3:42f3821c4e54 195 loaded = confStatus;
shimniok 3:42f3821c4e54 196
shimniok 0:a6a169de725f 197 return confStatus;
shimniok 9:39c0ff43332b 198 }