Diff: Config/Config.cpp

Revision:

9:39c0ff43332b

Parent:

3:42f3821c4e54

Child:

22:dc54ca6e6eec

--- a/Config/Config.cpp	Thu Nov 29 16:14:45 2018 +0000
+++ b/Config/Config.cpp	Thu Nov 29 16:20:25 2018 +0000
@@ -1,137 +1,185 @@
 #define MAXBUF 64
 
 #include "Config.h"
-#include "SDHCFileSystem.h"
+#include "SDFileSystem.h"
 #include "GeoPosition.h"
 #include "globals.h"
 #include "util.h"
 
-extern Serial pc;
+// Identifiers for each of the parameters
+#define CURB            "curb"
+#define WAYPOINT        "wpt"
+#define GPS             "gps"
+#define GYRO            "gyro"
+#define MAGNETOMETER    "mag"
+#define ACCELEROMETER   "accel"
+#define DECLINATION     "decl"
+#define NAVIGATION      "nav"
+#define STEER           "steer"
+#define SPEED           "speed"
+#define VEHICLE         "veh"
+#define ENCODER         "enc"
 
-// TODO: 3: mod waypoints to include speed after waypoint
 
 Config::Config():
-    loaded(false)
+    loaded(false),
+    intercept(0),
+    waypointDist(0),
+    brakeDist(0),
+    wptCount(0),
+    escMin(1300),
+    escZero(1300),
+    escMax(1300),
+    topSpeed(0),
+    turnSpeed(0),
+    startSpeed(0),
+    minRadius(0),
+    speedKp(0),
+    speedKi(0),
+    speedKd(0),
+    steerZero(0),
+    steerScale(0),
+    curbThreshold(0),
+    curbGain(0),
+    gyroScale(0),
+    // Data Bus Original Settings
+    wheelbase(0.280),
+    track(0.290),
+    tireCirc(0.321537),
+    encStripes(32)
 {
-    // not much to do here, yet.
+    for (int i=0; i < MAX_WPT; i++) {
+        wptTopSpeedAdj[i] = 0;
+        wptTurnSpeedAdj[i] = 0;
+    }
+    for (int i=0; i < 3; i++) {
+        magOffset[i] = 0;
+        magScale[i] = 0;
+    }
 }
 
 // load configuration from filesystem
-bool Config::load()
+bool Config::load(const char *filename)
 {
-    LocalFileSystem local("etc");         // Create the local filesystem under the name "local"
     FILE *fp;
     char buf[MAXBUF];   // buffer to read in data
     char tmp[MAXBUF];   // temp buffer
     char *p;
     double lat, lon;
+    float wTopSpeed, wTurnSpeed;
     bool declFound = false;
     bool confStatus = false;
-    
-    // Just to be safe let's wait
-    //wait(2.0);
 
     pc.printf("opening config file...\n");
     
-    fp = fopen("/etc/config.txt", "r");
+    fp = fopen(filename, "r");
     if (fp == 0) {
-        pc.printf("Could not open config.txt\n");
+        pc.puts("Could not open ");
+        pc.puts(filename);
+        pc.puts(" \n");
     } else {
         wptCount = 0;
-        declination = 0.0;
         while (!feof(fp)) {
             fgets(buf, MAXBUF-1, fp);
-            p = split(tmp, buf, MAXBUF, ',');           // split off the first field
-            switch (tmp[0]) {
-                case 'B' :
-                    p = split(tmp, p, MAXBUF, ',');     // threshold distance for curb avoid
-                    curbThreshold = cvstof(tmp);
-                    p = split(tmp, p, MAXBUF, ',');     // steering gain for curb avoid
-                    curbGain = cvstof(tmp);
-                    break;
-                case 'W' :                              // Waypoint
-                    p = split(tmp, p, MAXBUF, ',');     // split off the latitude to tmp
-                    lat = cvstof(tmp);
-                    p = split(tmp, p, MAXBUF, ',');     // split off the longitude to tmp
-                    lon = cvstof(tmp);
-                    if (wptCount < MAXWPT) {
-                        wpt[wptCount].set(lat, lon);
-                        wptCount++;
-                    }
-                    break;
-                case 'G' :                              // GPS
-                    p = split(tmp, p, MAXBUF, ',');
-                    gpsBaud = atoi(tmp);                // baud rate for gps
-                    p = split(tmp, p, MAXBUF, ',');
-                    gpsType = atoi(tmp);
-                    break;
-                case 'Y' :                              // Gyro Bias
-                    p = split(tmp, p, MAXBUF, ',');     // split off the declination to tmp
-                    gyroBias = (float) cvstof(tmp);
-                    break;
-                case 'D' :                              // Compass Declination
-                    p = split(tmp, p, MAXBUF, ',');     // split off the declination to tmp
-                    declination = (float) cvstof(tmp);
-                    declFound = true;
-                    break;
-                case 'N' :                                  // Navigation and Turning    
-                    p = split(tmp, p, MAXBUF, ',');
-                    interceptDist = (float) cvstof(tmp);    // intercept distance for steering algorithm
-                    p = split(tmp, p, MAXBUF, ',');
-                    waypointDist = (float) cvstof(tmp);     // distance before waypoint switch
-                    p = split(tmp, p, MAXBUF, ',');
-                    brakeDist = (float) cvstof(tmp);        // distance at which braking starts
+            p = split(tmp, buf, MAXBUF, ',');               // split off the first field
+
+            if (!strcmp(tmp, CURB)) {
+
+                p = split(tmp, p, MAXBUF, ',');             // threshold distance for curb avoid
+                curbThreshold = cvstof(tmp);
+                p = split(tmp, p, MAXBUF, ',');             // steering gain for curb avoid
+                curbGain = cvstof(tmp);
+
+            } else if (!strcmp(tmp, WAYPOINT)) {
+
+                p = split(tmp, p, MAXBUF, ',');             // split off the latitude to tmp
+                lat = cvstof(tmp);
+                p = split(tmp, p, MAXBUF, ',');             // split off the longitude to tmp
+                lon = cvstof(tmp);
+                p = split(tmp, p, MAXBUF, ',');             // split off the waypoint top speed
+                wTopSpeed = cvstof(tmp);
+                p = split(tmp, p, MAXBUF, ',');             // split off the waypoint turn speed
+                wTurnSpeed = cvstof(tmp);
+                if (wptCount < MAXWPT) {
+                    wpt[wptCount].set(lat, lon);            // set position
+                    wptTopSpeedAdj[wptCount] = wTopSpeed;   // set top speed adjust
+                    wptTurnSpeedAdj[wptCount] = wTurnSpeed; // set turn speed adjust
+                    wptCount++;
+                }
+
+            } else if (!strcmp(tmp, NAVIGATION)) {
+
+                p = split(tmp, p, MAXBUF, ',');
+                intercept = (float) cvstof(tmp);        // intercept distance for steering algorithm
+                p = split(tmp, p, MAXBUF, ',');
+                waypointDist = (float) cvstof(tmp);         // distance before waypoint switch
+                p = split(tmp, p, MAXBUF, ',');
+                brakeDist = (float) cvstof(tmp);            // distance at which braking starts
+                p = split(tmp, p, MAXBUF, ',');
+                minRadius = (float) cvstof(tmp);            // minimum turning radius
+
+            } else if (!strcmp(tmp, STEER)) {
+
+                p = split(tmp, p, MAXBUF, ',');
+                steerZero = cvstof(tmp);                    // servo center setting
+                p = split(tmp, p, MAXBUF, ',');
+                steerScale = cvstof(tmp);                   // steering angle conversion factor
+
+            } else if (!strcmp(tmp, SPEED)) {
+
+                p = split(tmp, p, MAXBUF, ',');
+                escMin = cvstof(tmp);                       // minimum esc (brake) setting
+                p = split(tmp, p, MAXBUF, ',');
+                escZero = cvstof(tmp);                      // esc center/zero setting
+                p = split(tmp, p, MAXBUF, ',');
+                escMax = cvstof(tmp);                       // maximum esc setting
+                p = split(tmp, p, MAXBUF, ',');
+                topSpeed = cvstof(tmp);                     // desired top speed
+                p = split(tmp, p, MAXBUF, ',');
+                turnSpeed = cvstof(tmp);                    // speed to use when turning
+                p = split(tmp, p, MAXBUF, ',');
+                startSpeed = cvstof(tmp);                   // speed to use at start
+                p = split(tmp, p, MAXBUF, ',');
+                speedKp = cvstof(tmp);                      // speed PID: proportional gain
+                p = split(tmp, p, MAXBUF, ',');
+                speedKi = cvstof(tmp);                      // speed PID: integral gain
+                p = split(tmp, p, MAXBUF, ',');
+                speedKd = cvstof(tmp);                      // speed PID: derivative gain
+
+            } else if (!strcmp(tmp, VEHICLE)) {
+                p = split(tmp, p, MAXBUF, ',');
+                wheelbase = cvstof(tmp);                    // vehicle wheelbase (front to rear axle)
+                p = split(tmp, p, MAXBUF, ',');
+                track = cvstof(tmp);                        // vehicle track width (left to right)
+            } else if (!strcmp(tmp, ENCODER)) {
+                p = split(tmp, p, MAXBUF, ',');
+                tireCirc = cvstof(tmp);                     // tire circumference
+                p = split(tmp, p, MAXBUF, ',');
+                encStripes = atoi(tmp);                     // ticks per revolution
+            } else if (!strcmp(tmp, GYRO)) {
+                p = split(tmp, p, MAXBUF, ',');             // split off the declination to tmp
+                gyroScale = cvstof(tmp);            // gyro scaling factor to deg/sec
+            } else if (!strcmp(tmp, GPS)) {
+                p = split(tmp, p, MAXBUF, ',');
+                gpsValidSpeed = cvstof(tmp);
+            } //if-else
+            /* else if (!strcmp(tmp, DECLINATION)) {
+                p = split(tmp, p, MAXBUF, ',');     // split off the declination to tmp
+                declination = (float) cvstof(tmp);
+                declFound = true;
+            } else if (!strcmp(tmp, MAGNETOMETER)) {
+                for (int i=0; i < 3; i++) {
                     p = split(tmp, p, MAXBUF, ',');
-                    minRadius = (float) cvstof(tmp);        // minimum turning radius
-                    break;
-                case 'M' :                              // Magnetometer
-                    for (int i=0; i < 3; i++) {
-                        p = split(tmp, p, MAXBUF, ',');
-                        magOffset[i] = (float) cvstof(tmp);
-                        pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]);
-                    }
-                    for (int i=0; i < 3; i++) {
-                        p = split(tmp, p, MAXBUF, ',');
-                        magScale[i] = (float) cvstof(tmp);
-                        pc.printf("magScale[%d]: %.2f\n", i, magScale[i]);
-                    }                    
-                case 'R' : // Steering configuration
-                    p = split(tmp, p, MAXBUF, ',');
-                    steerZero = cvstof(tmp);            // servo center setting
-                    p = split(tmp, p, MAXBUF, ',');
-                    steerGain = cvstof(tmp);            // steering angle multiplier
-                    p = split(tmp, p, MAXBUF, ',');
-                    steerGainAngle = cvstof(tmp);       // angle below which steering gain takes effect
-                    break;
-                case 'S' :                              // Throttle configuration
+                    magOffset[i] = (float) cvstof(tmp);
+                    pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]);
+                }
+                for (int i=0; i < 3; i++) {
                     p = split(tmp, p, MAXBUF, ',');
-                    escMin = atoi(tmp);                 // minimum esc (brake) setting
-                    p = split(tmp, p, MAXBUF, ',');     
-                    escZero = atoi(tmp);                // esc center/zero setting
-                    p = split(tmp, p, MAXBUF, ',');     
-                    escMax = atoi(tmp);                 // maximum esc setting
-                    p = split(tmp, p, MAXBUF, ',');   
-                    topSpeed = cvstof(tmp);             // desired top speed
-                    p = split(tmp, p, MAXBUF, ',');   
-                    turnSpeed = cvstof(tmp);            // speed to use within brake distance of waypoint
-                    p = split(tmp, p, MAXBUF, ',');
-                    startSpeed = cvstof(tmp);            // speed to use at start
-                    p = split(tmp, p, MAXBUF, ',');
-                    speedKp = cvstof(tmp);              // speed PID: proportional gain
-                    p = split(tmp, p, MAXBUF, ',');     
-                    speedKi = cvstof(tmp);              // speed PID: integral gain
-                    p = split(tmp, p, MAXBUF, ',');     
-                    speedKd = cvstof(tmp);              // speed PID: derivative gain
-    
-                    break;
-                case 'E' :
-                    p = split(tmp, p, MAXBUF, ',');     
-                    compassGain = (float) cvstof(tmp);  // not used (DCM)
-                    p = split(tmp, p, MAXBUF, ',');    
-                    yawGain = (float) cvstof(tmp);      // not used (DCM)
-                default :
-                    break;
-            } // switch
+                    magScale[i] = (float) cvstof(tmp);
+                    pc.printf("magScale[%d]: %.2f\n", i, magScale[i]);
+                }
+            }
+            */
         } // while
 
         // Did we get the values we were looking for?
@@ -147,4 +195,4 @@
     loaded = confStatus;
 
     return confStatus;
-}
+}
\ No newline at end of file