Revision 9:39c0ff43332b, committed 2018-11-29

Comitter:

shimniok

Date:

Thu Nov 29 16:20:25 2018 +0000

Parent:

8:aed35fb80b0f

Child:

10:3b588c648967

Commit message:

Deleted Actuators, modified config to latest

Changed in this revision

--- a/Actuators/Actuators.cpp	Thu Nov 29 16:14:45 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-#include "mbed.h"
-#include "Servo.h"
-#include "Config.h"
-
-extern Config config;
-
-Servo steering(p21);                    // Steering Servo
-Servo throttle(p22);                    // ESC
-int escMax=520;                         // Servo setting for max speed
-
-#define THROTTLE_CENTER 0.4
-
-void initSteering()
-{
-    if (config.loaded) {
-        // Setup steering servo
-        steering = config.steerZero;
-    } else {
-        steering = 0.4;
-    }
-    // TODO: 3 parameterize this in config file
-    steering.calibrate(0.005, 45.0); 
-}
-
-
-void initThrottle()
-{
-    if (config.loaded) {
-        throttle = (float)config.escZero/1000.0;
-    } else {
-        throttle = 0.410;
-    }
-    // TODO: 3 parameterize this in config file
-    throttle.calibrate(0.001, 45.0); 
-}
-
-void setThrottle(int value)
-{
-    throttle = ((float)value)/1000.0;
-}
-
-void setSteering(float steerAngle)
-{
-    // Convert steerAngle to servo value
-    // Testing determined near linear conversion between servo ms setting and steering angle
-    // up to 20*.  Assumes a particular servo library with range = 0.005
-    // In that case, f(SA) = servoPosition = 0.500 + SA/762.5
-    // between 20 and 24* the slope is approximately 475
-    // What if we ignore the linearity and just set to a max angle
-    // also range is 0.535-0.460 --> slope = 800
-    // steering = 0.500 + (double) steerAngle / 762.5;
-    //
-    // TODO: 1 parameterize through config
-    steering = 0.500 + (double) steerAngle / 808.0;
-}
-    

--- a/Actuators/Actuators.h	Thu Nov 29 16:14:45 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#ifndef __ACTUATORS_H
-#define __ACTUATORS_H
-
-/** Abstraction of steering and throttle control
- *
- */
-
-void initSteering(void);
-void initThrottle(void);
-void setThrottle(int value);
-void setSteering(float steerAngle);
-
-#endif
\ No newline at end of file

--- 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

--- a/UI/Buttons/Buttons.cpp	Thu Nov 29 16:14:45 2018 +0000
+++ b/UI/Buttons/Buttons.cpp	Thu Nov 29 16:20:25 2018 +0000
@@ -1,9 +1,9 @@
 #include "Buttons.h"
 #include "PinDetect.h"
 
-PinDetect nextButton(p14);
-PinDetect selectButton(p16);            // Input selectButton
-PinDetect prevButton(p15);
+PinDetect nextButton(BUTTONNEXT);
+PinDetect selectButton(BUTTONSELECT);
+PinDetect prevButton(BUTTONPREV);
 
 Buttons::Buttons(void): which(0), pressed(false)
 {

--- a/globals.h	Thu Nov 29 16:14:45 2018 +0000
+++ b/globals.h	Thu Nov 29 16:20:25 2018 +0000
@@ -14,7 +14,7 @@
 #include "Serial.h"
 #include "SerialGraphicLCD.h"
 
-extern Steering steering;
+//extern Steering steering;
 extern Serial pc;
 extern SerialGraphicLCD lcd;
 extern Buttons keypad;

--- a/mbed-rtos.lib	Thu Nov 29 16:14:45 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed-rtos/#5713cbbdb706