David Spillman
Crude navigation
Dependencies: GPS L3GD20 LSM303DLHC mbed PID
Revision 4:a397b44a0fe8, committed 2015-03-17
- Comitter:
- Spilly
- Date:
- Tue Mar 17 01:13:17 2015 +0000
- Parent:
- 3:ffa0e1429a72
- 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(°Gyro[0], °Gyro[1], °Gyro[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
/********************************************************************************************************************************/