Revision 4:a397b44a0fe8, committed 2015-03-17

Comitter:

Spilly

Date:

Tue Mar 17 01:13:17 2015 +0000

Parent:

3:ffa0e1429a72

Child:

5:40ac894e0fa7

Commit message:

Accel and magn offsets

Changed in this revision

--- a/LSM303DLHC.lib	Mon Mar 09 20:41:28 2015 +0000
+++ b/LSM303DLHC.lib	Tue Mar 17 01:13:17 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/bclaus/code/LSM303DLHC/#612f7d5a822d
+http://developer.mbed.org/users/Spilly/code/LSM303DLHC/#386547b2ee70

--- a/main.cpp	Mon Mar 09 20:41:28 2015 +0000
+++ b/main.cpp	Tue Mar 17 01:13:17 2015 +0000
@@ -13,18 +13,21 @@
 //Period in seconds of the the main loop
 #define PERIOD 0.5f
 //Period in seconds of PID loop
-#define RATE 0.1f
+#define RATE 0.05f
 #define PIDCYCLES 100
 //GPS
-GPS gps(D9, D7);
+GPS gps(PTC4, PTC3);
 
 //X-Bee
-Serial xBee(PTC15, PTC14);
+Serial xBee(PTD3, PTD2);       //boat
+//Serial xBee(PTC15, PTC14);   //car
 
 //PID
 //Kc, Ti, Td, interval
 PID controller(.1, .005, .01, RATE);
 
+Timer t2;
+
 //Enter new position here
 float goalPos[2] = {35.336020, -81.912420};
 
@@ -39,16 +42,6 @@
 
 int main()
 {
-    float motorSpeed = 0.5f;
-    //PID control of left and right motors based on input from gyro
-    //Goal is to have the vehicle go straight
-    controller.setInputLimits(-180, 180);
-    controller.setOutputLimits(-.5, .5);
-    //set mode to auto
-    controller.setMode(1);
-    //We want the difference to be zero
-    controller.setSetPoint(0);
-    
     xBee.baud(9600);
     
     xBee.printf("\nI'm Alive...\n");
@@ -62,23 +55,15 @@
         xBee.printf("Press any key to begin\n");
         wait(1);
     }
-    
-    //Test PID heading
-    while(1)
-    {
-        if(updateAngles())
-        {
-            makeVector();
-            //set heading to a fixed number for testing
-            float magDiff = whichWay(yaw, 0);
-            controller.setProcessValue(magDiff);
-            
-            motorSpeed = controller.compute();
-            goForward(0.5f + motorSpeed,0.5f - motorSpeed);
-            xBee.printf("heading = %f\tdiff = %f\tspeed = %f\n", yaw, magDiff, motorSpeed);
-            wait(RATE);
-        }
-    }
+    float motorSpeed = 0.5f;
+    //PID control of left and right motors based on input from gyro
+    //Goal is to have the vehicle go straight
+    controller.setInputLimits(-180, 180);
+    controller.setOutputLimits(-.5, .5);
+    //set mode to auto
+    controller.setMode(1);
+    //We want the difference to be zero
+    controller.setSetPoint(0);
     
     xBee.printf("attempting to get a fix\n");
     

--- a/modSensData.h	Mon Mar 09 20:41:28 2015 +0000
+++ b/modSensData.h	Tue Mar 17 01:13:17 2015 +0000
@@ -6,18 +6,46 @@
 L3GD20 gyro(PTE25, PTE24);
 LSM303DLHC compass(PTE25, PTE24);
 
-#define XBIAS 0
-#define YBIAS 0
-#define ZBIAS 0
-
 #define M_PI            3.1415926535897932384626433832795f
 #define TWO_PI          6.283185307179586476925286766559f
+#define RADTODEGREE     57.295779513082320876798154814105f
+#define GRAVITY       1.0f
+#define THRESHOLD 1.1f
 
-#define RADTODEGREE     57.295779513082320876798154814105f
+// "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+#define ACCEL_X_MIN ((float) -276)
+#define ACCEL_X_MAX ((float) 236)
+#define ACCEL_Y_MIN ((float) -261)
+#define ACCEL_Y_MAX ((float) 245)
+#define ACCEL_Z_MIN ((float) -284)
+#define ACCEL_Z_MAX ((float) 220)
 
-#define GRAVITY       1.0f
+// "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+#define MAGN_X_MIN (-0.370909f)
+#define MAGN_X_MAX (0.569091f)
+#define MAGN_Y_MIN (-0.516364f)
+#define MAGN_Y_MAX (0.392727f)
+#define MAGN_Z_MIN (-0.618367f)
+#define MAGN_Z_MAX (0.408163f)
 
-#define THRESHOLD 1.1f
+// Sensor calibration scale and offset values
+#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
+#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
+#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
+#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
+#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
+#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
+
+#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
+#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
+#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
+#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
+#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
+#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
+
+
+
+//calibrated reading = (reading - offset) * scale
 
 Timer t1;
 
@@ -42,6 +70,16 @@
 void readIMU()
 {
     compass.read(&accel[0], &accel[1], &accel[2], &magnetom[0], &magnetom[1], &magnetom[2]);
+    //Compensate magnetometer error
+    //magnetom[0] -= MAGN_X_OFFSET;
+    //magnetom[1] -= MAGN_Y_OFFSET;
+    //magnetom[2] -= MAGN_Z_OFFSET;
+    
+    //Compensate accelerometer error
+    //accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    //accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+    //accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+    
     gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
 }
 
@@ -107,20 +145,6 @@
   roll = atan2(temp2[1], temp2[2]);
 }
 
-
-/********************************************************************************************************************************/
-//                                                      compensateAcc
-/********************************************************************************************************************************/
-/*
-void compensateAcc() 
-{
-    // Compensate accelerometer error
-    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
-    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
-    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
-}
-*/
-
 /********************************************************************************************************************************/
 //                                                      rotateBTI
 /********************************************************************************************************************************/