Revision 15:01fb4916a5cd, committed 2018-11-29

Comitter:

shimniok

Date:

Thu Nov 29 17:11:34 2018 +0000

Parent:

14:86ee4d840f6b

Child:

16:4c4b75824efc

Commit message:

temp fix for setSteering() compile err in shell.cpp; updated logging class

Changed in this revision

--- a/Actuators/Steering/Steering.cpp	Thu Nov 29 16:59:39 2018 +0000
+++ b/Actuators/Steering/Steering.cpp	Thu Nov 29 17:11:34 2018 +0000
@@ -1,23 +1,65 @@
 #include "mbed.h"
+#include "globals.h"
 #include "Steering.h"
 #include "math.h"
 #include "util.h"
 
-/** create a new steering calculator for a particular vehicle
- *
- */
-Steering::Steering(float wheelbase, float track):
-    _wheelbase(wheelbase),
-    _track(track),
-    _intercept(2.0)
+Steering::Steering(PinName pin):
+    _steering(pin),
+    _scale(0),
+    _track(0),
+    _wheelbase(0),
+    _intercept(0)
 {
 }
 
-void Steering::setIntercept(float intercept)
+
+void Steering::initSteering()
 {
+    _steering = 0.4;
+    // TODO: 3 parameterize this in config file ?
+    _steering.calibrate(0.005, 45.0);
+}
+
+void Steering::setScale(float scale) {
+    _scale = scale;
+}
+
+void Steering::setIntercept(float intercept) {
     _intercept = intercept;
 }
 
+void Steering::setWheelbase(float wheelbase) {
+    _wheelbase = wheelbase;
+}
+
+void Steering::setTrack(float track) {
+    _track = track;
+}
+
+// TODO 3 convert steering to Servo2 library
+void Steering::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;
+    //
+    _steering = 0.500 + (double) steerAngle * _scale;
+}
+
+
+float Steering::operator =(float steerAngle)
+{
+    setSteering(steerAngle);
+    return steerAngle;
+}
+
+
 /** Calculate a steering angle based on relative bearing
  *
  */
@@ -46,14 +88,14 @@
 
     // Compute |radius| based on intercept distance and specified angle with extra gain to
     // overcome steering slop, misalignment, sidehills, etc.
-    radius = _intercept / ( 2 * sin(angle_radians(theta)) );
+    radius = _intercept / ( 2 * sin(toRadians(theta)) );
 
     if (minRadius > 0) {
         if (radius < minRadius) radius = minRadius;
     }
 
     // Now calculate steering angle based on wheelbase and track width
-    SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
+    SA = toDegrees(asin(_wheelbase / (radius - _track/2)));
     // The above ignores the effect of speed on required steering angle.
     // Even when under the limits of traction, understeer means more angle
     // is required to achieve a turn at higher speeds than lower speeds.
@@ -122,7 +164,7 @@
     // Compute a circle that is tangential to bot heading and intercepts bot
     // and goal point (LAx,LAy), the intercept circle. Then compute the steering
     // angle to trace that circle. (x,y because 0 deg points up not right)
-    float brg = clamp360( angle_degrees(atan2(LAx-Rx,LAy-Ry)) );
+    float brg = clamp360( toDegrees(atan2(LAx-Rx,LAy-Ry)) );
     //if (brg >= 360.0) brg -= 360.0;
     //if (brg < 0) brg += 360.0;
     // would be nice to add in some noise to heading info
@@ -138,11 +180,11 @@
     // when subtracting track/2.0, so just take absolute value and multiply sign
     // later on
     sign = (relBrg < 0) ? -1 : 1;
-    float radius = _intercept/fabs(2*sin(angle_radians(relBrg)));
+    float radius = _intercept/fabs(2*sin(toRadians(relBrg)));
     // optionally, limit radius min/max
     // Now compute the steering angle to achieve the circle of 
     // Steering angle is based on wheelbase and track width
-    return ( sign * angle_degrees(asin(_wheelbase / (radius - _track/2.0))) );
+    return ( sign * toDegrees(asin(_wheelbase / (radius - _track/2.0))) );
 }
 
 
@@ -200,7 +242,7 @@
     }
 
     // Now calculate steering angle based on wheelbase and track width
-    SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
+    SA = toDegrees(asin(_wheelbase / (radius - _track/2)));
 
     return SA;
-}
\ No newline at end of file
+}

--- a/Actuators/Steering/Steering.h	Thu Nov 29 16:59:39 2018 +0000
+++ b/Actuators/Steering/Steering.h	Thu Nov 29 17:11:34 2018 +0000
@@ -2,6 +2,9 @@
 #define __STEERING_H
 
 #include "globals.h"
+#include "devices.h"
+#include "Servo.h"
+#include "Config.h"
 
 /** A class for managing steering angle calculations based on current and desired heading
  *  and specified intercept distance along the new path.  
@@ -11,19 +14,55 @@
 class Steering
 {
   public:
-    /** create a new steering calculator
+    /** create a new steering calculator */
+    Steering(PinName pin);
+
+    /** Initalize steering actuator (servo)
+     *
+     */
+    void initSteering(void);
+
+
+    /** Set the track width (left to right contact patch distance)
      *
-     * @param wheelbase vehicle wheelbase
-     * @param track vehicle track width
-     * @param intercept new course intercept distance
+     * @param track is the vehicle track width
      */
-    Steering(float wheelbase, float track);
+    void setTrack(float track);
+
+
+    /** Set the wheelbase (front to rear axle distance)
+     *
+     * @param wheelbase is the vehicle wheelbase
+     */
+    void setWheelbase(float wheelbase);
+
 
     /** set intercept distance
      * @param intercept distance along new course at which turn arc will intercept
      */
     void setIntercept(float intercept);
 
+
+    /** set steering scale factor to convert between steering angle and servo output
+     * @param scale is the scale factor
+     */
+    void setScale(float scale);
+
+
+    /** Convert steerAngle to servo value
+     *
+     * Testing determined near linear conversion between servo ms setting and steering angle
+     * up to ~20*.
+     *
+     * @param steerAngle is the steering angle, measured at front wheels, averaged
+     */
+    void setSteering(float steerAngle);
+
+
+    /**  Shorthand for the write function */
+    float operator= (float steerAngle);
+
+
     /** convert course change to average steering angle
      * assumes Ackerman steering, with track and wheelbase
      * and course intercept distance specified.
@@ -35,6 +74,7 @@
      */ 
     float calcSA(float theta);
 
+
     /** convert course change to average steering angle
      * assumes Ackerman steering, with track and wheelbase
      * and course intercept distance specified. Also, |radius| of turn is limited to limit
@@ -47,28 +87,42 @@
      */ 
     float calcSA(float theta, float limit);
 
+
     /** compute steering angle based on pure pursuit algorithm
      */
     float purePursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy);
     
+
     /** compute steering angle based on a simpler path pursuit variant of pure pursuit
      */
     float pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy);
 
+
     /** Compute cross track error given last waypoint, next waypoint, and robot coordinates
      * @returns cross track error
      */
     float crossTrack(float Bx, float By, float Ax, float Ay, float Cx, float Cy);
-    
-  private:
+
 
-    inline static float angle_radians(float deg) {return (PI/180.0)*deg;}
+    /** Convert degrees to radians
+     * @param deg degrees to convert
+     * @return radians
+     */
+    inline static float toRadians(float deg) {return (PI/180.0)*deg;}
+
 
-    inline static float angle_degrees(float rad) {return (180/PI)*rad;}
+    /** Convert radians to degrees
+     * @param rad radians to convert
+     * @return degrees
+     */
+    inline static float toDegrees(float rad) {return (180/PI)*rad;}
 
-    float _wheelbase;
-    float _track;
-    float _intercept;
+  private:
+    Servo _steering;                // Steering Servo
+    float _scale;                   // Steering scale factor
+    float _track;                   // vehicle track used for steering intercept calc
+    float _wheelbase;               // vehicle wheelbase used for steering intercept calc
+    float _intercept;               // circle intercept distance
 };
 
-#endif
\ No newline at end of file
+#endif

--- a/UI/shell.cpp	Thu Nov 29 16:59:39 2018 +0000
+++ b/UI/shell.cpp	Thu Nov 29 17:11:34 2018 +0000
@@ -16,7 +16,7 @@
 char buf[MAXBUF];
 char path[MAXBUF];
 int status;
-bool debug=false;
+bool _debug=false;
 bool done=false;
 
 typedef struct {
@@ -249,7 +249,7 @@
         basename[k-(sep+1)] = path[k];
         basename[k-sep] = 0;    
     }
-    //if (debug) fprintf(stdout, "d:<%s> b:<%s>\n", dirname, basename);
+    //if (_debug) fprintf(stdout, "d:<%s> b:<%s>\n", dirname, basename);
 
 
 }
@@ -258,7 +258,7 @@
  * lists files in the current working directory, 4 columns
  */
 int dols(char *arg) {
-    //if (debug) fprintf(stdout, "%s\n", cwd);
+    //if (_debug) fprintf(stdout, "%s\n", cwd);
     DIR *d;
     struct dirent *p;
 
@@ -435,10 +435,10 @@
 }
 
 /** dodebug
- * toggle the debug state variable
+ * toggle the _debug state variable
  */
 int dodebug(char *arg) {
-    debug = !debug;
+    _debug = !_debug;
 
     return 0;
 }
@@ -503,7 +503,8 @@
     fputs("angle=", stdout);
     printFloat(stdout, v, 4);
     fputs(" servo=", stdout);
-    setSteering(v);
+    // TODO fix setSteering call
+    //setSteering(v);
     // TODO 4 printFloat(stdout, getSteering(), 4);
     fputc('\n', stdout);
     return 0;

--- a/logging/logging.cpp	Thu Nov 29 16:59:39 2018 +0000
+++ b/logging/logging.cpp	Thu Nov 29 17:11:34 2018 +0000
@@ -1,37 +1,16 @@
+#include "SystemState.h"
+#include "globals.h"
 #include "logging.h"
-#include "SDHCFileSystem.h"
+#include "SDFileSystem.h"
 #include "SerialGraphicLCD.h"
 
-extern Serial pc;
-extern SerialGraphicLCD lcd;
+// TODO 2 set up logging out of low priority interrupt handler
 
-SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
+//SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
 static FILE *logp;
 
-void clearState( SystemState *s )
-{
-    s->millis = 0;
-    s->current = s->voltage = 0.0;
-    s->g[0] = s->g[1] = s->g[2] = 0;
-    s->gyro[0] = s->gyro[1] = s->gyro[2] = 0;
-    s->gTemp = 0;
-    s->a[0] = s->a[1] = s->a[2] = 0;
-    s->m[0] = s->m[1] = s->m[2] = 0;
-    s->gHeading = s->cHeading = 0.0;
-    //s->roll = s->pitch = s->yaw =0.0;
-    s->gpsLatitude = s->gpsLongitude = s->gpsCourse_deg = s->gpsSpeed_mps = s->gpsHDOP = 0.0;
-    //s->gpsLatitude2 = s->gpsLongitude2 = s->gpsCourse_deg2 = s->gpsSpeed_mps2 = s->gpsHDOP2 = 0.0;
-    s->lrEncDistance = s->rrEncDistance = 0.0;
-    s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
-    s->estHeading = s->estLatitude = s->estLongitude = 0.0;
-    //s->estNorthing = s->estEasting =
-    s->estX = s->estY = 0.0;
-    s->nextWaypoint = 0;
-    s->bearing = s->distance = 0.0;
-}
-
 Timer logtimer;
-extern int bufCount;
+//extern int bufCount;
 
 /*
 void logData( const SystemState s ) {
@@ -130,97 +109,106 @@
 
 // If I use arduino style print routines, logging takes ~1000 / ~8000 usec
 // the big sprintf takes ~ 700-750 usec all by itself
-void logData( const SystemState s )
+void logData( SystemState *s )
 {
     //char buf[256];
-    unsigned int t1, t2;
-    logtimer.start();
-    logtimer.reset();
-    t1 = logtimer.read_us();
-    printInt(logp, s.millis);
-    fputc(',',logp);
-    printFloat(logp, s.current, 2);
-    fputc(',',logp);
-    printFloat(logp, s.voltage, 2);
-    fputc(',',logp);
-    for (int q=0; q < 3; q++) {
-        printFloat(logp, s.gyro[q], 6);
+    //unsigned int t1, t2;
+    //logtimer.start();
+    //logtimer.reset();
+    //t1 = logtimer.read_us();
+
+    if (s) {
+        printInt(logp, s->millis);
+        fputc(',',logp);
+        printFloat(logp, s->current, 2);
+        fputc(',',logp);
+        printFloat(logp, s->voltage, 2);
         fputc(',',logp);
-    }
-    printInt(logp, s.gTemp);
-    fputc(',',logp);
-    for (int q=0; q < 3; q++) {
-        printInt(logp, s.a[q]);
+        for (int q=0; q < 3; q++) {
+            printFloat(logp, s->gyro[q], 6);
+            fputc(',',logp);
+        }
+        printInt(logp, s->gTemp);
         fputc(',',logp);
-    }
-    /*
-    for (int q=0; q < 3; q++) {
-        printInt(logp, s.m[q]);
+        for (int q=0; q < 3; q++) {
+            printInt(logp, s->a[q]);
+            fputc(',',logp);
+        }
+        /*
+        for (int q=0; q < 3; q++) {
+            printInt(logp, s->m[q]);
+            fputc(',',logp);
+        }
+        */
+        printFloat(logp, s->gHeading, 2);
         fputc(',',logp);
-    }
-    */
-    printFloat(logp, s.gHeading, 2);
-    fputc(',',logp);
 
-    // GPS 1
-    fprintf(logp, "%.7f,%.7f,", s.gpsLatitude, s.gpsLongitude);
-    //printFloat(logp, s.gpsLatitude, 7);
-    //fputc(',',logp);
-    //printFloat(logp, s.gpsLongitude, 7);
-    //fputc(',',logp);
-    printFloat(logp, s.gpsCourse_deg, 2);
-    fputc(',',logp);
-    printFloat(logp, s.gpsSpeed_mps, 2);
-    fputc(',',logp);
-    printFloat(logp, s.gpsHDOP, 1);
-    fputc(',',logp);
-    printInt(logp, s.gpsSats);
-    fputc(',',logp);
-    // Encoders
-    printFloat(logp, s.lrEncDistance, 7);
-    fputc(',',logp);
-    printFloat(logp, s.rrEncDistance, 7);
-    fputc(',',logp);
-    printFloat(logp, s.lrEncSpeed, 2);
-    fputc(',',logp);
-    printFloat(logp, s.rrEncSpeed, 2);
-    fputc(',',logp);
-    printFloat(logp, s.encHeading, 2);
-    fputc(',',logp);
-    // Estimates
-    printFloat(logp, s.estHeading, 2);
-    fputc(',',logp);
-    printFloat(logp, s.estLagHeading, 2);
-    fputc(',',logp);
-    printFloat(logp, s.estLatitude,  7);
-    fputc(',',logp);
-    printFloat(logp, s.estLongitude, 7);
-    fputc(',',logp);
-    printFloat(logp, s.estX, 4);
-    fputc(',',logp);
-    printFloat(logp, s.estY, 4);
-    fputc(',',logp);
-    // Nav
-    printInt(logp, s.nextWaypoint);
-    fputc(',',logp);
-    printFloat(logp, s.bearing, 2);
-    fputc(',',logp);
-    printFloat(logp, s.distance, 3);
-    fputc(',',logp);
-    printFloat(logp, s.steerAngle, 3);
-    fputc(',',logp);
-    printFloat(logp, s.errHeading, 3);
-    fputc(',',logp);
-    fputc('\n',logp);
+        // GPS 1
+        fprintf(logp, "%.7f,%.7f,", s->gpsLatitude, s->gpsLongitude);
+        //printFloat(logp, s->gpsLatitude, 7);
+        //fputc(',',logp);
+        //printFloat(logp, s->gpsLongitude, 7);
+        //fputc(',',logp);
+        printFloat(logp, s->gpsCourse_deg, 2);
+        fputc(',',logp);
+        printFloat(logp, s->gpsSpeed_mps, 2);
+        fputc(',',logp);
+        printFloat(logp, s->gpsHDOP, 1);
+        fputc(',',logp);
+        printInt(logp, s->gpsSats);
+        fputc(',',logp);
+        // Encoders
+        printFloat(logp, s->lrEncDistance, 7);
+        fputc(',',logp);
+        printFloat(logp, s->rrEncDistance, 7);
+        fputc(',',logp);
+        printFloat(logp, s->lrEncSpeed, 2);
+        fputc(',',logp);
+        printFloat(logp, s->rrEncSpeed, 2);
+        fputc(',',logp);
+        printFloat(logp, s->encHeading, 2);
+        fputc(',',logp);
+        // Estimates
+        printFloat(logp, s->estHeading, 2);
+        fputc(',',logp);
+        printFloat(logp, s->estLagHeading, 2);
+        fputc(',',logp);
+        printFloat(logp, s->estLatitude,  7);
+        fputc(',',logp);
+        printFloat(logp, s->estLongitude, 7);
+        fputc(',',logp);
+        printFloat(logp, s->estX, 4);
+        fputc(',',logp);
+        printFloat(logp, s->estY, 4);
+        fputc(',',logp);
+        // Nav
+        printInt(logp, s->nextWaypoint);
+        fputc(',',logp);
+        printFloat(logp, s->bearing, 2);
+        fputc(',',logp);
+        printFloat(logp, s->distance, 3);
+        fputc(',',logp);
+        printFloat(logp, s->steerAngle, 3);
+        fputc(',',logp);
+        printFloat(logp, s->errHeading, 3);
+        fputc(',',logp);
+        printFloat(logp, s->LABrg, 2);
+        fputc(',',logp);
+        printFloat(logp, s->LAx, 4);
+        fputc(',',logp);
+        printFloat(logp, s->LAy, 4);
+        fputc('\n',logp);
+        fflush(logp);
 
-    t2 = logtimer.read_us();
-    //fprintf(stdout, "%d\n", t2-t1);
+        //t2 = logtimer.read_us();
+        //fprintf(stdout, "%d\n", t2-t1);
+    }
 
     return;
 }
 
 
-FILE *openlog(char *prefix)
+FILE *openlog(const char *prefix)
 {
     FILE *fp = 0;
     char myname[64];
@@ -228,7 +216,8 @@
     pc.printf("Opening file...\n");
 
     while (fp == 0) {
-        if ((fp = fopen("/log/test.txt", "w")) == 0) {
+        sprintf(myname, "%s/test.txt", LOGDIR);
+        if ((fp = fopen(myname, "w")) == 0) {
             pc.printf("Waiting for filesystem to come online...");
             wait(0.200);
             lcd.pos(0,1);
@@ -238,7 +227,7 @@
     fclose(fp);
 
     for (int i = 0; i < 1000; i++) {
-        sprintf(myname, "/log/%s%03d.csv", prefix, i);
+        sprintf(myname, "%s/%s%03d.csv", LOGDIR, prefix, i);
         if ((fp = fopen(myname, "r")) == 0) {
             break;
         } else {
@@ -250,7 +239,7 @@
         pc.printf("file write failed: %s\n", myname);
     } else {
 
-        // TODO -- set error message, get rid of writing to terminal
+        // TODO 3 set error message, get rid of writing to terminal
 
         //status = true;
         pc.printf("opened %s for writing\n", myname);
@@ -281,4 +270,4 @@
 void closeLogfile(void)
 {
     if (logp) fclose(logp);
-}
\ No newline at end of file
+}

--- a/logging/logging.h	Thu Nov 29 16:59:39 2018 +0000
+++ b/logging/logging.h	Thu Nov 29 17:11:34 2018 +0000
@@ -6,10 +6,11 @@
 #include "mbed.h"
 #include "SystemState.h"
 
-FILE *openlog(char *prefix);
+#define LOGDIR "/log"
+
+FILE *openlog(const char *prefix);
 bool initLogfile(void);
-void clearState( SystemState *s );
-void logData( SystemState s );
+void logData( SystemState *s );
 void closeLogfile(void);
 
-#endif
\ No newline at end of file
+#endif

--- a/updater.h	Thu Nov 29 16:59:39 2018 +0000
+++ b/updater.h	Thu Nov 29 17:11:34 2018 +0000
@@ -5,9 +5,18 @@
  * called at 100Hz by a timer interrupt
  */
 
+/** initialize throttle to center position */
+void initThrottle(void);
+
+/** initialize steering to center position */
+void initSteering(void);
+
 /** attach the update routine to Ticker interrupt */
 void startUpdater(void);
 
+/** detach the update routine from Ticker interrupt */
+void stopUpdater(void);
+
 /** Returns the elapsed time taken by the updater routine on its most recent run */
 int getUpdateTime(void);
 
@@ -20,6 +29,12 @@
 /** Tells the updater to re-initialize the navigation state */
 void restartNav(void);
 
+/** Sets the desired speed of the rover */
+void setSpeed(float speed);
+
+/** Sets the steering angle */
+void setSteering(float steerAngle);
+
 /** The function that is called at 100Hz. It reads sensors, performs estimation, and controls the robot */
 void update(void);