Revision 3:42f3821c4e54, committed 2013-06-07

Comitter:

shimniok

Date:

Fri Jun 07 14:45:46 2013 +0000

Parent:

2:fbc6e3cf3ed8

Child:

4:2567b5292249

Commit message:

Working version 6/6/2013. Heading estimation may not be quite perfect yet but seems the major estimation bugs are fixed now.

Changed in this revision

--- a/Actuators/Actuators.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/Actuators/Actuators.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -12,8 +12,12 @@
 
 void initSteering()
 {
-    // Setup steering servo
-    steering = config.steerZero;
+    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); 
 }
@@ -21,7 +25,11 @@
 
 void initThrottle()
 {
-    throttle = (float)config.escZero/1000.0;
+    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); 
 }

--- a/Actuators/Steering/Steering.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/Actuators/Steering/Steering.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -1,7 +1,7 @@
+#include "mbed.h"
 #include "Steering.h"
 #include "math.h"
-
-#include "mbed.h"
+#include "util.h"
 
 /** create a new steering calculator for a particular vehicle
  *
@@ -105,13 +105,13 @@
 
 float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
 {
-    float SA;
     // Leg vector
     float Lx = Cx - Ax;
     float Ly = Cy - Ay;
     // Robot vector
     float Rx = Bx - Ax;
     float Ry = By - Ay;
+    float sign = 1;
 
     // Find the goal point, a projection of the bot vector onto the current leg, moved ahead
     // along the path by the lookahead distance
@@ -122,34 +122,27 @@
     // 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 = angle_degrees(atan2(LAx-Rx,LAy-Ry));
-    if (brg >= 360.0) brg -= 360.0;
-    if (brg < 0) brg += 360.0;
+    float brg = clamp360( angle_degrees(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
-    float relBrg = brg - hdg;
-    if (relBrg < -180.0) relBrg += 360.0;
-    if (relBrg >= 180.0) relBrg -= 360.0;
-    // The equation peaks out at 90* so clamp theta artifically to 90, so that
-    // if theta is actually > 90, we select max steering
-    if (fabs(relBrg) > 90.0) relBrg = 90.0;
+    float relBrg = clamp180(brg - hdg);
+    // The steering angle equation actually peaks at relBrg == 90 so just clamp to this
+    if (relBrg > 89.5) {
+        relBrg = 89.5;
+    } else if (relBrg < -89.5) {
+        relBrg = -89.5;
+    }
     // Compute radius based on intercept distance and specified angle
-    float radius = _intercept/(2*sin(angle_radians(fabs(relBrg))));
+    // The sin equation will produce a negative radius, which causes problems
+    // 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)));
     // optionally, limit radius min/max
     // Now compute the steering angle to achieve the circle of 
     // Steering angle is based on wheelbase and track width
-    SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
-    // I haven't had time to work out why the equation is asymmetrical, that is, 
-    // negative angle produces slightly less steering angle.
-    if (relBrg < 0) SA = -SA; 
-    
-    /*
-    if (++skip >= 20) {
-        fprintf(stdout, "R(%.4f,%.4f) A(%.4f,%.4f) C(%.4f,%.4f) LA(%.4f,%.4f) h=%.2f b=%.2f rb=%.2f sa=%.2f\n", Bx, By, Ax, Ay, Cx, Cy, LAx, LAy, hdg, brg, relBrg, SA);
-        skip = 0;
-    }
-    */
-        
-    return SA;
+    return ( sign * angle_degrees(asin(_wheelbase / (radius - _track/2.0))) );
 }
 
 

--- a/Config/Config.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/Config/Config.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -10,7 +10,8 @@
 
 // TODO: 3: mod waypoints to include speed after waypoint
 
-Config::Config()
+Config::Config():
+    loaded(false)
 {
     // not much to do here, yet.
 }
@@ -143,5 +144,7 @@
     if (fp != 0)
         fclose(fp);
 
+    loaded = confStatus;
+
     return confStatus;
 }

--- a/Config/Config.h	Thu Jun 06 13:40:23 2013 +0000
+++ b/Config/Config.h	Fri Jun 07 14:45:46 2013 +0000
@@ -11,7 +11,7 @@
         Config();
 
         bool load();
-        
+        bool loaded;            // has the config been loaded yet?
         float interceptDist;    // used for course correction steering calculation
         float waypointDist;     // distance threshold to waypoint
         float brakeDist;        // braking distance

--- a/Estimation/kalman.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/Estimation/kalman.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -1,9 +1,10 @@
 #include "mbed.h"
 #include "Matrix.h"
+#include "util.h"
 
 #define DEBUG 1
 
-#define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360))
+//#define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360))
 
 /*
  * Kalman Filter Setup
@@ -176,17 +177,14 @@
      ***********************************************************************/
     float Hx[2];
     Matrix_Multiply(2,2,1, Hx, H, xp);
-
+    
     //Matrix_print(2,2, H, "6. H");
     //Matrix_print(2,1, x, "6. x");
     //Matrix_print(2,1, Hx, "6. Hx");
     
     float zHx[2];
     Matrix_Subtract(2,1, zHx, z, Hx);
-
-    // At this point we need to be sure to correct heading to -180 to 180 range
-    if (zHx[0] > 180.0)   zHx[0] -= 360.0;
-    if (zHx[0] <= -180.0) zHx[0] += 360.0;
+    zHx[0] = clamp180(zHx[0]);
 
     //Matrix_print(2,1, z, "6. z");
     //Matrix_print(2,1, zHx, "6. zHx");
@@ -198,10 +196,7 @@
     //Matrix_print(2,1, KzHx, "6. KzHx");
     
     Matrix_Add(2,1, x, xp, KzHx);
-
-    // Clamp to 0-360 range
-    while (x[0] < 0) x[0] += 360.0;
-    while (x[0] >= 360.0) x[0] -= 360.0;
+    x[0] = clamp360(x[0]);    // Clamp to 0-360 range
 
     //Matrix_print(2,1, x, "6. x");
 
@@ -223,6 +218,5 @@
     Matrix_Copy(2, 2, P, P2);
 
     //Matrix_print(2,2, P, "7. P");
-
     return x[0];
 }

--- a/SystemState.h	Thu Jun 06 13:40:23 2013 +0000
+++ b/SystemState.h	Fri Jun 07 14:45:46 2013 +0000
@@ -55,6 +55,7 @@
     unsigned int millis;
     float current, voltage;
     int g[3];
+    float gyro[3];
     int gTemp;
     int a[3];
     int m[3];
@@ -79,7 +80,7 @@
     float bearing;
     float distance;
     float gbias;
-    float errAngle;
+    float errHeading;
     float steerAngle;
 } SystemState;
 

--- a/logging/logging.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/logging/logging.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -13,6 +13,7 @@
     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;
@@ -143,7 +144,7 @@
     printFloat(logp, s.voltage, 2);
     fputc(',',logp);
     for (int q=0; q < 3; q++) {
-        printInt(logp, s.g[q]);
+        printFloat(logp, s.gyro[q], 6);
         fputc(',',logp);
     }
     printInt(logp, s.gTemp);
@@ -208,6 +209,8 @@
     fputc(',',logp);
     printFloat(logp, s.steerAngle, 3);
     fputc(',',logp);
+    printFloat(logp, s.errHeading, 3);
+    fputc(',',logp);
     fputc('\n',logp);
 
     t2 = logtimer.read_us();

--- a/main.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/main.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -193,11 +193,12 @@
     // totally jacks up everything (noise?)
     initSteering();
     initThrottle();
-    // initFlasher();                                   // Initialize autonomous mode flasher
+    // initFlasher();                       // Initialize autonomous mode flasher
     
     // Send data back to the PC
     pc.baud(115200);
     fprintf(stdout, "Data Bus 2013\n");
+    while (pc.readable()) pc.getc();        // flush buffer on reset
 
     display.init();
     display.status("Data Bus 2013");

--- a/updater.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/updater.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -27,11 +27,11 @@
 //#define LOG_SKIP 1  // 50ms, 20hz, log entry entered into fifo
 
 // The following is for main loop at 10ms = 100hz
-#define HDG_LAG 100
-#define CTRL_SKIP 5 // 50ms (20hz), control update
-#define MAG_SKIP 2  // 20ms (50hz), magnetometer update
-#define LOG_SKIP 2  // 20ms (50hz), log entry entered into fifo
-//#define LOG_SKIP 5  // 50ms (20hz), log entry entered into fifo
+#define HDG_LAG 40      // Number of update steps that the GPS report is behind realtime
+#define CTRL_SKIP 5     // 50ms (20hz), control update
+#define MAG_SKIP 2      // 20ms (50hz), magnetometer update
+#define LOG_SKIP 2      // 20ms (50hz), log entry entered into fifo
+//#define LOG_SKIP 5    // 50ms (20hz), log entry entered into fifo
 
 Ticker sched;                           // scheduler for interrupt driven routines
 
@@ -82,7 +82,7 @@
 float dt;                               // dt for the KF
 float estLagHeading = 0;                   // lagged heading estimate
 //GeoPosition lagHere;                  // lagged position estimate; use here as the current position estimate
-float errAngle;                         // error between gyro hdg estimate and gps hdg estimate
+float errHeading;                         // error between gyro hdg estimate and gps hdg estimate
 float gyroBias=0;                       // exponentially weighted moving average of gyro error
 float Ag = (2.0/(1000.0+1.0));          // Gyro bias filter alpha, gyro; # of 10ms steps
 float Kbias = 0.995;            
@@ -227,6 +227,8 @@
     sensors.Read_Power();
 
     sensors.Read_Encoders(); 
+    // really need to do some filtering on the speed
+    //nowSpeed = 0.8*nowSpeed + 0.2*sensors.encSpeed; 
     nowSpeed = sensors.encSpeed;
 
     sensors.Read_Gyro(); 
@@ -301,10 +303,7 @@
     history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
     history[now].gyro = sensors.gyro[_z_];  // current raw gyro data
     history[now].dt = dt; // current dt, to address jitter
-    history[now].hdg = history[prev].hdg + dt*(history[now].gyro - gyroBias); // compute current heading from current gyro
-    clamp(history[now].hdg, 0, 360.0);
-    //if (history[now].hdg >= 360.0) history[now].hdg -= 360.0;
-    //if (history[now].hdg < 0)      history[now].hdg += 360.0;
+    history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro - gyroBias) ); // compute current heading from current gyro
     float r = PI/180 * history[now].hdg;
     history[now].x = history[prev].x + history[now].dist * sin(r);    // update current position
     history[now].y = history[prev].y + history[now].dist * cos(r);
@@ -365,27 +364,21 @@
 
         // Low pass filter the error correction.  Multiplying by 0.01, it takes HDG_LAG updates to correct a 
         // consistent error; that's 0.10s/0.01 = 1 sec.  0.005 is 2 sec, 0.0025 is 4 sec, etc.
-        errAngle = (estLagHeading - history[lag].hdg); 
-        // low pass filter error angle:
-        clamp(errAngle, -180.0, 180.0);
-        //if (errAngle > 180.0) errAngle -= 360.0;
-        //if (errAngle <= -180.0) errAngle += 360.0;
-        errAngle *= 0.01; // multiply only after clamping to -180:180 range.
+        errHeading = clamp180(estLagHeading - history[lag].hdg) * 0.01;  // lopass filter error angle
 
-        //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, estLagHeading, history[lag].hdg, estLagHeading - history[lag].hdg, errAngle);
-        float cosA = cos(errAngle * PI / 180.0);
-        float sinA = sin(errAngle * PI / 180.0);
+        //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, estLagHeading, history[lag].hdg, estLagHeading - history[lag].hdg, errHeading);
+        float cosA = cos(errHeading * PI / 180.0);
+        float sinA = sin(errHeading * PI / 180.0);
         // Update position & heading from history[lag] through history[now]
         int i = lag;
         // TODO 2 parameterize heading lag -- for uBlox it seems to be ~ 600ms, for Venus, about 1000ms
         for (int j=0; j < HDG_LAG; j++) {
             //fprintf(stdout, "i=%d n=%d l=%d\n", i, now, lag);
             // Correct gyro-calculated headings from past to present including history[lag].hdg
-            history[i].hdg += errAngle;
-            clamp(history[i].hdg, 0, 360.0);
+            history[i].hdg = clamp360(history[i].hdg + errHeading);
             //if (history[i].hdg >= 360.0) history[i].hdg -= 360.0;
             //if (history[i].hdg < 0) history[i].hdg += 360.0;
-            // Rotate x, y by errAngle around pivot point; no need to rotate pivot point (j=0)
+            // Rotate x, y by errHeading around pivot point; no need to rotate pivot point (j=0)
             if (j > 0) {
                 float dx = history[i].x-history[lag].x;
                 float dy = history[i].y-history[lag].y;
@@ -409,11 +402,6 @@
     bearing = cartHere.bearingTo(config.cwpt[nextWaypoint]);
     distance = cartHere.distanceTo(config.cwpt[nextWaypoint]);
     float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]);
-    double relativeBrg = bearing - history[now].hdg;
-    // if correction angle is < -180, express as negative degree
-    clamp(relativeBrg, -180.0, 180.0);
-    //if (relativeBrg < -180.0) relativeBrg += 360.0;
-    //if (relativeBrg > 180.0)  relativeBrg -= 360.0;
 
     // if within config.waypointDist distance threshold move to next waypoint
     // TODO 3 figure out how to output feedback on wpt arrival external to this function
@@ -446,38 +434,24 @@
     //////////////////////////////////////////////////////////////////////////////
     // OBSTACLE DETECTION & AVOIDANCE
     //////////////////////////////////////////////////////////////////////////////
-    // TODO: 1 limit steering angle based on object detection ?
+    // TODO 2 limit steering angle based on object detection ?
     // or limit relative brg perhaps?
-    // TODO: 1 add vision obstacle detection and filtering
-    // TODO: 1 add ranger obstacle detection and filtering/fusion with vision
+    // TODO 2 add vision obstacle detection and filtering
+    // TODO 2 add ranger obstacle detection and filtering/fusion with vision
 
 
     //////////////////////////////////////////////////////////////////////////////
     // CONTROL UPDATE
     //////////////////////////////////////////////////////////////////////////////
 
-    // TODO: 1 improve the steering algorithm to take cross-track error into account
-
     if (--control_count == 0) {
   
         steerAngle = steerCalc.pathPursuitSA(history[now].hdg, history[now].x, history[now].y,
                                              config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
                                              config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
         
-        // TODO 3: eliminate pursuit config item
-        /*
-        if (config.usePP) {
-            steerAngle = steerCalc.purePursuitSA(history[now].hdg, 
-                                                 history[now].x, history[now].y,
-                                                 config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
-                                                 config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
-        } else {
-            steerAngle = steerCalc.calcSA(relativeBrg, config.minRadius); // use the configured minimum turn radius
-        }
-        */
-        
-        
         // Apply gain factor for near straight line
+        // TODO 3 figure out a better, continuous way to deal with steering gain
         if (fabs(steerAngle) < config.steerGainAngle) steerAngle *= config.steerGain;
 
         // Curb avoidance
@@ -562,6 +536,7 @@
     for (int i=0; i < 3; i++) {
         state[inState].m[i] = sensors.m[i];
         state[inState].g[i] = sensors.g[i];
+        state[inState].gyro[i] = sensors.gyro[i];
         state[inState].a[i] = sensors.a[i];
     }
     state[inState].gTemp = sensors.gTemp;
@@ -581,7 +556,7 @@
     state[inState].distance = distance;
     state[inState].nextWaypoint = nextWaypoint;
     state[inState].gbias = gyroBias;
-    state[inState].errAngle = biasErrAngle;
+    state[inState].errHeading = errHeading;
     //state[inState].leftRanger = sensors.leftRanger;
     //state[inState].rightRanger = sensors.rightRanger;
     //state[inState].centerRanger = sensors.centerRanger;

--- a/util.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/util.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -1,12 +1,24 @@
+#include <math.h>
+
 /** 
  * Clamp a value (angle) between min (non-inclusive) and max (inclusive)
  * e.g. clamp(v, 0, 360) or clamp(v, -180, 180)
  */
-float clamp(float v, float min, float max) 
+float clamp(float v, float min, float max, bool flip) 
 {
+    float i;
+    float f;
     float mod = (max - min);
-    if (v >= max) float -= mod;
-    if (v < min) float += mod;
+
+    f = modff((v/mod), &i) * mod;
+    if (flip) {
+        if (f > max) f -= mod;
+        if (f <= min) f += mod;
+    } else {
+        if (f < min) f += mod;
+        if (f >= max) f -= mod;
+    }
+    return f;
 }
 
 // convert character to an int

--- a/util.h	Thu Jun 06 13:40:23 2013 +0000
+++ b/util.h	Fri Jun 07 14:45:46 2013 +0000
@@ -3,17 +3,22 @@
 
 /** Utility routines */
 
-#define clamp360(x) \
-                while ((x) >= 360.0) (x) -= 360.0; \
-                while ((x) < 0) (x) += 360.0;
-#define clamp180(x) ((x) - floor((x)/360.0) * 360.0 - 180.0);
+#define clamp360(x) clamp((x), 0, 360.0, false)
+#define clamp180(x) clamp((x), -180.0, 180.0, true)
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
 #endif
 
-/** Clamp value between min (non-inclusive) and max (inclusive) */
-float clamp(float v, float min, float max);
+/**
+ * Clamp value between min and max. Specify which of the two are inclusive
+ * @param v is the value to clamp
+ * @param min is the minimum value, inclusive if flip==false, exclusive otherwise
+ * @param max is the maximum value, inclusive if flip==true, exclusive otherwise
+ * @param flip determines whether min or max is inclusive
+ * @return the clamped value
+ */
+float clamp(float v, float min, float max, bool flip);
 
 /** Convert char to integer */
 int ctoi(char c);