David Spillman
Crude navigation
Dependencies: GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem
Fork of
GPSNavigation
by 
David Spillman
Revision 12:273479524c71, committed 2015-04-29
- Comitter:
- Spilly
- Date:
- Wed Apr 29 18:07:43 2015 +0000
- Parent:
- 11:1b34319671eb
- Child:
- 13:17f04a55c6e2
- Commit message:
- Updated comments
Changed in this revision
--- a/Actuator.h Mon Apr 27 18:10:52 2015 +0000
+++ b/Actuator.h Wed Apr 29 18:07:43 2015 +0000
@@ -37,6 +37,7 @@
ENA = valueOne;
}
+//calculate an equal distance from acuator center point that the acutator can physically move to without hitting limit switch
float calcEnds(float center, float max, float min)
{
float upperRange = max - center;
--- a/GPS.lib Mon Apr 27 18:10:52 2015 +0000 +++ b/GPS.lib Wed Apr 29 18:07:43 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/Spilly/code/GPS2/#2476bea153a1 +http://developer.mbed.org/users/Spilly/code/GPS2/#083f6fe8e4c4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUDataAndFilters.h Wed Apr 29 18:07:43 2015 +0000
@@ -0,0 +1,374 @@
+/*************************************************************************************************************************************************************/
+// Created by: Ryan Spillman
+//
+// Last updated 4/4/2015
+//
+// Contains several functions such as calculations for roll, pitch, and yaw (tilt compensated compass)
+// Also contains various filtering and calibration functions
+//
+// Requires L3GD20 gyro and LSM303DLHC magnetometer and accelerometer
+/*************************************************************************************************************************************************************/
+
+#include "mbed.h"
+#include "math.h"
+#include "L3GD20.h"
+#include "LSM303DLHC.h"
+
+#define IMUDataAndFilters_h
+
+L3GD20 gyro(PTE25, PTE24);
+LSM303DLHC compass(PTE25, PTE24);
+
+#define ALPHA_H 0.2f //Heading alpha (variable for low pass filter)
+#define ALPHA_A 0.1f //Heading accelerometer (variable for low pass filter)
+#define EARTHRADIUS 6378100.000000000000000000f //Radius of the earth in meters (DO NOT MODIFY)
+#define MAGN_PERIOD (1 / 15.0f) //magnetometer polling period (15 Hz) must manually update hardware ODR registers if chaging this value
+#define ACCEL_PERIOD (1 / 50.0f) //accelerometer polling period (50 Hz) must manually update hardware ODR registers if chaging this value
+#define GYRO_SAMPLE_PERIOD 0.01f //gyro sample period in seconds
+#define M_PI 3.1415926535897932384626433832795f
+#define TWO_PI 6.283185307179586476925286766559f
+#define RADTODEGREE 57.295779513082320876798154814105f
+#define DEGREETORAD 0.01745329251994329576923690768489f
+#define GRAVITY 1.0f
+#define CALIBRATION_COUNT 64
+#define AVG_COUNT 10
+
+// "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
+#define ACCEL_X_MIN -1.000000000f
+#define ACCEL_X_MAX 1.023437500f
+#define ACCEL_Y_MIN -1.015625000f
+#define ACCEL_Y_MAX 1.023437500f
+#define ACCEL_Z_MIN -1.023437500f
+#define ACCEL_Z_MAX 0.960937500f
+
+// "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)
+
+// 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)
+//not sure what the "normal" value is, thus not using scale for magnetometer
+//#define MAGN_X_SCALE (??? / (MAGN_X_MAX - MAGN_X_OFFSET))
+//#define MAGN_Y_SCALE (??? / (MAGN_Y_MAX - MAGN_Y_OFFSET))
+//#define MAGN_Z_SCALE (??? / (MAGN_Z_MAX - MAGN_Z_OFFSET))
+
+
+float magnetom[3] = {0,0,0};
+float accel[3] = {0,0,0};
+float degGyro[3] = {0,0,0};
+float prevDegGyro[3]= {0,0,0};
+float gyroOffset[3] = {0,0,0};
+float roll = 0, pitch = 0, yaw = 0;
+float totalAccel = 0;
+float normalizedGravity;
+
+float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
+
+float prevYaw = 0;
+
+/********************************************************************************************************************************/
+// Low Pass
+/********************************************************************************************************************************/
+
+float lowPass(float input, float output, int select)
+{
+ //Magnetometer Heading alpha
+ if(select == 0)
+ {
+ if (output == 0.0000001f) return input;
+
+ output = output + ALPHA_H * (input - output);
+
+ return output;
+ }
+ //Accelerometer alpha
+ else if(select == 1)
+ {
+ if (output == 0.0000001f) return input;
+
+ output = output + ALPHA_A * (input - output);
+
+ return output;
+ }
+ //GPS heading alpha
+ else
+ {
+ if (output == 0.0000001f) return input;
+
+ output = output + ALPHA_A * (input - output);
+
+ return output;
+ }
+}
+
+/********************************************************************************************************************************/
+// getMagn
+/********************************************************************************************************************************/
+
+void getMagn()
+{
+ //LSM303DLHC/LSM303DLHC.cpp
+ compass.ReadMagnOnly(&magnetom[0], &magnetom[1], &magnetom[2]);
+
+ //Compensate magnetometer error
+ //magnetom[0] -= MAGN_X_OFFSET;
+ //magnetom[1] -= MAGN_Y_OFFSET;
+ //magnetom[2] -= MAGN_Z_OFFSET;
+}
+/********************************************************************************************************************************/
+// getAccel
+/********************************************************************************************************************************/
+
+void getAccel()
+{
+ //LSM303DLHC/LSM303DLHC.cpp
+ compass.ReadAccOnly(&accel[0], &accel[1], &accel[2]);
+
+ //Compensate accelerometer error
+ //TODO: verify compensation calculations
+ 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;
+
+
+ for(int i = 0; i <= 3; i++)
+ {
+ lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
+ }
+}
+
+/********************************************************************************************************************************/
+// Integrate Yaw With Gyro Data
+/********************************************************************************************************************************/
+//not used
+void gyroIntegral()
+{
+ float yawDiff = yaw - prevYaw;
+ float absYawDiff = sqrt(yawDiff * yawDiff);
+ //vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
+ float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * MAGN_PERIOD);
+ float absGyroInt = sqrt(gyroInt * gyroInt);
+ prevDegGyro[2] = degGyro[2];
+ if(absYawDiff > absGyroInt)
+ {
+ yaw = prevYaw + gyroInt;
+ }
+}
+
+/********************************************************************************************************************************/
+// Compass Heading
+/********************************************************************************************************************************/
+//see http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf for more info on these formulas
+void Compass_Heading()
+{
+ float mag_x;
+ float mag_y;
+ float cos_roll;
+ float sin_roll;
+ float cos_pitch;
+ float sin_pitch;
+
+ //saves a few processor cycles by calculating and storing values
+ cos_roll = cos(roll);
+ sin_roll = sin(roll);
+ cos_pitch = cos(pitch);
+ sin_pitch = sin(pitch);
+
+ // Tilt compensated magnetic field X
+ mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+
+ // Tilt compensated magnetic field Y
+ mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+
+ // Magnetic Heading
+ yaw = atan2(-mag_x, mag_y);
+
+ //make negative angles positive
+ if(yaw < 0) yaw = yaw + TWO_PI;
+ //yaw = yaw + M_PI;
+ yaw = yaw * RADTODEGREE;
+}
+
+/********************************************************************************************************************************/
+// getAttitude
+/********************************************************************************************************************************/
+//see http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf for more info on these formulas
+void getAttitude()
+{
+ float temp1[3];
+ float temp2[3];
+ float xAxis[3] = {1.0f, 0.0f, 0.0f};
+
+ // GET PITCH
+ // Using y-z-plane-component/x-component of gravity vector
+ pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
+
+ //printf("P = %f", pitch);
+
+ // GET ROLL
+ // Compensate pitch of gravity vector
+ temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
+ temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
+ temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
+
+ temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+ temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+ temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+
+ // Since we compensated for pitch, x-z-plane-component equals z-component:
+ roll = atan2(temp2[1], temp2[2]);
+}
+
+/********************************************************************************************************************************/
+// Filtered_Attitude()
+/********************************************************************************************************************************/
+
+void Filtered_Attitude()
+{
+ float temp1[3];
+ float temp2[3];
+ float xAxis[3] = {1.0f, 0.0f, 0.0f};
+
+ // GET PITCH
+ // Using y-z-plane-component/x-component of gravity vector
+ pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
+
+ //printf("P = %f", pitch);
+
+ // GET ROLL
+ // Compensate pitch of gravity vector
+ temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
+ temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
+ temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
+
+ temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+ temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+ temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+
+ // Since we compensated for pitch, x-z-plane-component equals z-component:
+ roll = atan2(temp2[1], temp2[2]);
+}
+
+/********************************************************************************************************************************/
+// updateAngles
+/********************************************************************************************************************************/
+
+void updateAngles()
+{
+ Filtered_Attitude();
+ Compass_Heading();
+}
+
+/********************************************************************************************************************************/
+// normalizeGravity
+/********************************************************************************************************************************/
+//not used
+void normalizeGravity()
+{
+ float accumulator = 0;
+ int sampleCount = 0;
+
+
+ //We'll take a certain number of samples and then average them to calculate the average
+ while (sampleCount < AVG_COUNT)
+ {
+ getAccel();
+ //add current sample to previous samples
+ accumulator += sqrt((accel[0] * accel[0]) + (accel[1] * accel[1]) + (accel[2] * accel[2]));
+ sampleCount++;
+ //Make sure the IMU has had enough time to take a new sample.
+ wait(0.06);
+ }
+
+
+ //divide by number of samples to get average offset
+ normalizedGravity = accumulator / AVG_COUNT;
+
+}
+/********************************************************************************************************************************/
+// gyroAvg
+/********************************************************************************************************************************/
+//not used
+void gyroAvg()
+{
+ float accumulator[3] = {0,0,0};
+ int sampleCount = 0;
+
+
+ //We'll take a certain number of samples and then average them to calculate the offset
+ while (sampleCount < AVG_COUNT)
+ {
+ //get gyro data
+ gyro.read(°Gyro[0], °Gyro[1], °Gyro[2]);
+ //add current sample to previous samples
+ accumulator[2] += degGyro[2];
+ sampleCount++;
+ //Make sure the gyro has had enough time to take a new sample.
+ wait(GYRO_SAMPLE_PERIOD);
+ }
+
+
+ //divide by number of samples to get average offset
+ degGyro[2] = accumulator[2] / AVG_COUNT;
+
+}
+
+/********************************************************************************************************************************/
+// gyroCal
+/********************************************************************************************************************************/
+//not used
+void gyroCal()
+{
+ float accumulator[3] = {0,0,0};
+ int sampleCount = 0;
+
+
+ //We'll take a certain number of samples and then average them to calculate the offset
+ while (sampleCount < CALIBRATION_COUNT)
+ {
+ float gyroData[3];
+ //Make sure the gyro has had enough time to take a new sample.
+ wait(GYRO_SAMPLE_PERIOD);
+ //get gyro data
+ gyro.read(&gyroData[0],&gyroData[1],&gyroData[2]);
+ for(int i = 0; i < 3; i++)
+ {
+ //add current sample to previous samples
+ accumulator[i] += gyroData[i];
+ }
+ sampleCount++;
+ }
+
+ for(int i = 0; i < 3; i++)
+ {
+ //divide by number of samples to get average offset
+ gyroOffset[i] = accumulator[i] / CALIBRATION_COUNT;
+ }
+}
+
+/********************************************************************************************************************************/
+// Only Get Gyro Data
+/********************************************************************************************************************************/
+//not used
+void gyroOnly()
+{
+ gyro.read(°Gyro[0], °Gyro[1], °Gyro[2]);
+ //compensate for gyro offset
+ for(int i=0;i<=3;i++)
+ {
+ degGyro[i] -= gyroOffset[i];
+ }
+}
\ No newline at end of file
--- a/PID.lib Mon Apr 27 18:10:52 2015 +0000 +++ b/PID.lib Wed Apr 29 18:07:43 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/Spilly/code/PID/#b7326da8243b +http://developer.mbed.org/users/Spilly/code/PID/#61d47c7736b5
--- a/TrollingMotor.h Mon Apr 27 18:10:52 2015 +0000
+++ b/TrollingMotor.h Wed Apr 29 18:07:43 2015 +0000
@@ -16,7 +16,6 @@
void goForward()
{
-
//Are we changing states?
if(prevState == 2)
{
--- a/main.cpp Mon Apr 27 18:10:52 2015 +0000
+++ b/main.cpp Wed Apr 29 18:07:43 2015 +0000
@@ -20,7 +20,7 @@
// Requires a serial to usb adapter to connect an X-Bee to a PC
// Set both X-Bees up for 115200 baud
// Use TeraTerm (or other serial program) to read and send data over X-Bees
-//
+// Set TeraTerm new line character from CR (carrage return) to LF (line feed)
// Program starts by prompting user to press any key
// Once user presses a key, the program waits for a DGPS fix (can be set by changing "FIX")
// Once the program sees a DGPS fix, manual mode is enabled
@@ -56,11 +56,20 @@
//
/*************************************************************************************************************************************************************************************************/
+//unmodified
+//mbed folder
#include "mbed.h"
+//SDFileSystem folder
+#include "SDFileSystem.h"
+
+//modified
+//GPS folder
#include "GPS.h"
+//PID folder
#include "PID.h"
-#include "SDFileSystem.h"
-#include "modSensData.h"
+
+//from scratch
+#include "IMUDataAndFilters.h"
#include "navigation.h"
#include "Actuator.h"
#include "TrollingMotor.h"
@@ -82,24 +91,32 @@
#define headKc 1.0f //directly proportional
#define headTi 0.0f //a larger number makes the integral have less affect on the output
#define headTd 0.0f //a smaller number makes the derivative have less affect on the output
+
+//PID/PID.cpp
PID headingPID(headKc, headTi, headTd, MAGN_PERIOD); //Kc, Ti, Td, interval
-AnalogIn battery(A0);
-AnalogIn pot(A1);
-
-Serial xBee(PTB11, PTB10); //UART 3
-GPS gps(PTC15, PTC14); //UART 4
-
+//mbed classes
Timer headingTime;
Timer acc;
Timer magn;
Timer inputTimer;
Timer loopTimer;
-
-FILE *fr; //file pointer for SD card
+AnalogIn battery(A0); //analog input from +12v side of linear actuator potentiometer (uses voltage divider)
+AnalogIn pot(A1); //analog input from the wiper of linear actuator potentionmeter (uses voltage divider)
+DigitalOut ldo(PTC10); //Controls 3.3V LDO v-reg powering MTK3339 (GPS) 1 = GPS powered on and 0 = GPS powered down
+DigitalOut green(D3); //Light output
+DigitalOut white(PTB9); //Light output
+Serial xBee(PTB11, PTB10); //UART 3
-//On board microSD
-SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd", PTE6, SDFileSystem::SWITCH_POS_NO, 50000000);
+//GPS/GPS.cpp
+GPS gps(PTC15, PTC14); //UART 4
+
+//Not sure where "FILE" is defined
+FILE *way; //file pointer for waypoints on SD card
+FILE *data; //file pointer for datalog
+
+//SDFileSystem/SDFileSystem.h
+SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd", PTE6, SDFileSystem::SWITCH_POS_NO, 50000000); //On board microSD
/***************************************************Prototype functions****************************************************************************************************************************/
void getDist(double posZero, double posOne, double curPos[2]);
@@ -107,35 +124,35 @@
/***************************************************End of prototype functions*********************************************************************************************************************/
/***************************************************Global Variables*******************************************************************************************************************************/
-double polarVector[2] = {0,0.0000001f};
-float manualSpeed = 0.5f;
-double curPos[2] = {0,0};
+//TODO: rewrite polar vector functions to allow elimination of global variables
-//waypoint data is overwritten by data from SD card
-double goalPos[10][2] = { {35.339289, -81.913164},
- {35.338943, -81.911024},
- {35.339289, -81.913164},
- {35.338943, -81.911024},
- {35.339289, -81.913164},
- {35.338943, -81.911024},
- {35.339289, -81.913164},
- {35.338943, -81.911024},
- {35.339289, -81.913164},
- {35.338943, -81.911024}
- };
+double polarVector[2] = {0,0.0000001f}; //poalarVector[0] = magnitude of vector, polarVector[1] = angle in degrees of vector
/*************************************************************************************************************************************************************************************************/
// MAIN
/*************************************************************************************************************************************************************************************************/
int main()
-{
+{
int wayPtNum = 0, mode = 0, adjustMode = 0;
float magDiff = 0;
float batVoltage = 0.0f, potVoltage = 0.0f, voltRatio = 0.0f;
float curSet = 0.0f, prevSet = 0.29f;
float filtered = 0.0000001f;
+ double curPos[2] = {0,0};
+ double goalPos[10][2]; //positions are initially read from SD card
+ //when a position is recorded in record mode, the previously stored position on the SD card is overwritten with the new position
+
+ //set the initial state of the lights
+ green = 1;
+ white = 0;
+ //turn the GPS 3.3V LDO v-reg on
+ ldo = 1;
+ //wait for the GPS unit to boot
+ wait(1);
+
+ //set xBee baud rate
xBee.baud(115200);
xBee.printf("\nI'm Alive...\n");
xBee.printf("Press any key to begin\n");
@@ -143,17 +160,18 @@
//wait for keypress to begin
while(!xBee.readable());
+ //Get goal positions from SD card
//start of SD card read
- fr = fopen ("/sd/GPS_CORDS.txt", "rt");
+ way = fopen ("/sd/GPS_CORDS.txt", "r");
xBee.printf("Reading SD Card Please Wait\n");
for(int x = 0; x<=9; x++)
{
- fscanf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fscanf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
xBee.printf("waypoint %d = %f,%f\n", x, goalPos[x][0], goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
//end of SD card read
//initialize magnetometer, accelerometer
@@ -176,28 +194,30 @@
}
else xBee.printf("Waiting for DGPS fix\tcurrent fix = no fix\n");
}
+
//get IMU data and calculate the tilt compensated compass
- getAccel();
- getMagn();
- updateAngles();
+ getAccel(); //IMUDataAndFilters.h
+ getMagn(); //IMUDataAndFilters.h
+ updateAngles(); //IMUDataAndFilters.h
xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
xBee.printf("dist %f\theading %f\n", polarVector[0], polarVector[1]);
xBee.printf("\n\nstarting main loop\n");
- //PID control of left and right motors based on input from tilt compensated compass
- //Goal is to have the vehicle go straight
+ //set input constraints (heading difference)
headingPID.setInputLimits(-180, 180);
//set proportional output limits based on physical limits of actuator and mounting error
- float distFromCenter = calcEnds(CENTER_RATIO, MAX_RATIO, MIN_RATIO);
+ float distFromCenter = calcEnds(CENTER_RATIO, MAX_RATIO, MIN_RATIO); //navigation.h
+ //set output constraints (linear actuator max and min)
headingPID.setOutputLimits((CENTER_RATIO - distFromCenter), (CENTER_RATIO + distFromCenter));
//set mode to auto
headingPID.setMode(0);
- //We want the difference to be zero
+ //We want the difference between actual heading and the heading needed to be zero
headingPID.setSetPoint(0);
+ //start timers
loopTimer.start();
headingTime.start();
acc.start();
@@ -211,43 +231,62 @@
if(mode == 0)
{
- if(gps.getData())
- {
- //gps.parseData();
- //xBee.printf("lat %f\tlon %f\tHDOP %f\tfix %d\tsat %d\tspd %f\n", gps.degLat, gps.degLon, gps.HDOP, gps.fixtype, gps.satellites, gps.speed);
- //GPSTimer.reset();
- }
- //This moves actuator to the requested position
- //This is proportional feedback only
+ //checks to see if all three NEMA sentences from the GPS UART has been received
+ gps.getData();
+
+ //check timer
if(loopTimer.read() > RATE)
{
+ //TODO: put this in a function
+
+ //This moves actuator to the requested position
+ //This is proportional feedback ONLY
+
+ //read analog input from wiper on potentiometer on linear actuator
potVoltage = pot.read();
+ //read analog input from battery voltage
batVoltage = battery.read();
- //voltage = voltage * 3.3f;
+ //calculate ratio of the two (using a ratio prevents battery voltage fluctuation from affecting actual position)
voltRatio = potVoltage / batVoltage;
+ //calculate the absolute value of how far off from requested actuator position we are (saves a few processor cycles)
float absDiff = sqrt((prevSet - voltRatio) * (prevSet - voltRatio));
+ //are we off enough to care? if so, stop moving the actuator
if(absDiff <= RATIO_TOLERANCE)
{
turnStop(1.0f, 1.0f);
//xBee.printf("done\n");
}
+
+ //do we need to go further right?
else if((prevSet - voltRatio) >= 0)
{
turnRight(1.0f, 1.0f);
//xBee.printf("turning right\n");
}
+
+ //do we need to go further left?
else
{
turnLeft(1.0f, 1.0f);
//xBee.printf("turning left\n");
}
- xBee.printf("battery = %f\tpot = %f\tratio = %f\tset %f\tHDOP = %f\tfix = %f\n", (batVoltage * VOLT_MULT), (potVoltage* VOLT_MULT), voltRatio, prevSet, gps.HDOP, gps.fixtype);
+ xBee.printf("battery = %f\tpot = %f\tratio = %f\tset %f\tHDOP = %f\tfix = %d\n", (batVoltage * VOLT_MULT), (potVoltage* VOLT_MULT), voltRatio, prevSet, gps.HDOP, gps.fixtype);
+
+ //toggle lights
+ green = !green;
+ white = !white;
+
+ //reset timer to zero
loopTimer.reset();
}
+
//check to see if data is available on xBee
if(xBee.readable())
{
+
+ //TODO: put this in a function
+
char recChar = xBee.getc();
//change to autonomous mode
@@ -264,6 +303,15 @@
goStop();
mode = 3;
}
+ else if(recChar == '<')
+ {
+ xBee.printf("Power cycling GPS\nPlease wait\n");
+ goStop();
+ ldo = 0;
+ wait(0.5);
+ ldo = 1;
+ wait(0.5);
+ }
else if(recChar == '1')
{
xBee.printf("stop\n");
@@ -344,18 +392,22 @@
/*********************************************************************************************************************************************************************************************/
// autonomous mode
/*********************************************************************************************************************************************************************************************/
-
+ //default trolling motor state when entering autonomous mode is off (user must press "w" to go forward)
if(mode == 1)
{
//check xBee
if(xBee.readable())
{
+ //TODO: put this in a function
+
char recChar = xBee.getc();
+ //stop
if(recChar == '1')
{
xBee.printf("stop\n");
goStop();
}
+ //go forward
if(recChar == 'w')
{
xBee.printf("forward\n");
@@ -381,8 +433,11 @@
polarVector[1] = polarVector[1] - 1;
xBee.printf("reduced angle %f\n", polarVector[1]);
}
+
+ //increments settings based on adjust mode
else if(recChar == '+')
{
+ //adjust waypoint
if(adjustMode == 0)
{
if(wayPtNum != 9)
@@ -395,6 +450,7 @@
xBee.printf("maximum waypoint reached\n");
}
}
+ //increment proportional gain of heading PID
else if(adjustMode == 1)
{
float curKc = headingPID.getPParam();
@@ -404,6 +460,7 @@
headingPID.setTunings(curKc, curTi, curTd);
xBee.printf("Kc set to %f\n", curKc);
}
+ //increment integral gain of heading PID
else if(adjustMode == 2)
{
float curKc = headingPID.getPParam();
@@ -413,6 +470,7 @@
headingPID.setTunings(curKc, curTi, curTd);
xBee.printf("Ti set to %f\n", curTi);
}
+ //increment derivative gain of heading PID
else if(adjustMode == 3)
{
float curKc = headingPID.getPParam();
@@ -423,6 +481,7 @@
xBee.printf("Td set to %f\n", curTd);
}
}
+ //decrements setting based on adjust mode
else if(recChar == '-')
{
if(adjustMode == 0)
@@ -437,6 +496,7 @@
xBee.printf("minimum waypoint reached\n");
}
}
+ //decrement proportional gain of heading PID
else if(adjustMode == 1)
{
float curKc = headingPID.getPParam();
@@ -446,6 +506,7 @@
headingPID.setTunings(curKc, curTi, curTd);
xBee.printf("Kc set to %f\n", curKc);
}
+ //decrement integral gain of heading PID
else if(adjustMode == 2)
{
float curKc = headingPID.getPParam();
@@ -455,6 +516,7 @@
headingPID.setTunings(curKc, curTi, curTd);
xBee.printf("Ti set to %f\n", curTi);
}
+ //decrement derivative gain of heading PID
else if(adjustMode == 3)
{
float curKc = headingPID.getPParam();
@@ -465,7 +527,7 @@
xBee.printf("Td set to %f\n", curTd);
}
}
- //change current waypoint number
+ //change or reset current waypoint number
else if(recChar == 'r')
{
goStop();
@@ -475,64 +537,92 @@
while(!xBee.readable());
char tempWS[2];
tempWS[0] = xBee.getc();
+
+ //press "r" again to reset waypoint number to zero
if(tempWS[0] == 'r')
{
wayPtNum = 0;
}
+
+ //else enter the number of waypoint desired
else
{
sscanf(tempWS, "%d", &wayPtNum);
xBee.printf("waypoint is now %d\n", wayPtNum);
}
}
+ //change adjust mode
else if(recChar == 'p')
{
xBee.printf("To set adjust mode:\nEnter w to adjust waypoint number\nEnter c to adjust Kc\nEnter i to adjust Ti\nEnter d to adjust Td\nEnter z to exit\n");
while(!xBee.readable());
char recCharTemp = xBee.getc();
+
+ //set adjust to edit waypoints
if(recCharTemp == 'w')
{
adjustMode = 0;
}
+
+ //set adjust to edit proportional gain
else if(recCharTemp == 'c')
{
adjustMode = 1;
xBee.printf("Adjust mode set to Kc\tEnter + to increment and - to decrement Kc\n");
}
+
+ //set adjust to edit integral gain
else if(recCharTemp == 'i')
{
adjustMode = 2;
xBee.printf("Adjust mode set to Ti\tEnter + to increment and - to decrement Ti\n");
}
+
+ //set adjust to edit derivative gain
else if(recCharTemp == 'd')
{
adjustMode = 3;
xBee.printf("Adjust mode set to Td\tEnter + to increment and - to decrement Td\n");
}
+ //if any other key is pressed no change to adjust mode is made
else
{
xBee.printf("No changes made\n");
}
}
}
+ //if no xBee data is received
else
{
+
+ //TODO: put this in a function
+
+ //checks to see if all three NEMA sentences from the GPS UART has been received
if(gps.getData())
{
double tempPos[2] = {0,0};
+ //store most recent gps location in a temporary variable (using temporary variables because GPS data is accumulated in a low pass filter)
tempPos[0] = gps.degLat;
tempPos[1] = gps.degLon;
+
+ //calculate the magnitude of the vector
getDist(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos);
+
+ //calculate the angle of the vector
getAngle(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos, 0);
- //xBee.printf("dist %f\tMagDiff %f\tHDOP = %f\n", polarVector[0], magDiff, gps.HDOP);
+ //checks if the magnitude of distance from goal position is less than threshold for "arriving"
if(polarVector[0] <= ARRIVED)
{
xBee.printf("Goal Position %d reached!\n", wayPtNum);
wait(1);
+
+ //increment waypoint number
wayPtNum ++;
- if(wayPtNum >= 6)
+
+ //we only have ten waypoints so we mus stop at ten
+ if(wayPtNum >= 10)
{
xBee.printf("Final Position Reached!\nShutting down\n");
goStop();
@@ -543,27 +633,41 @@
{
//flush heading PID data since we have a new heading
headingPID.reset();
+
+ //calculate the angle of the vector
getAngle(goalPos[wayPtNum ][0],goalPos[wayPtNum][1], goalPos[wayPtNum - 1], 1);
+
xBee.printf("Moving to Goal Position %d\theading = %f\n", wayPtNum, polarVector[1]);
}
}
}
+ //time to read accelerometer?
if(acc.read() >= ACCEL_PERIOD)
{
+ //get accelerometer data
getAccel();
+
+ //reset timer to zero
acc.reset();
}
- //Heading PID
+
+ //time to read magnatometer and calculate heading PID?
if(magn.read() >= MAGN_PERIOD)
{
- getMagn();
- updateAngles();
- filtered = lowPass(yaw, filtered, 0);
- magDiff = whichWay(filtered, 0);
+ //get magnatometer data
+ getMagn(); //IMUDataAndFilters.h
+ updateAngles(); //IMUDataAndFilters.h
+ filtered = lowPass(yaw, filtered, 0); //IMUDataAndFilters.h
+ magDiff = whichWay(filtered, 0); //navigation.h
+
+ //give PID input
headingPID.setProcessValue(-magDiff);
+
+ //get output from PID
curSet = headingPID.compute();
- xBee.printf("ratio = %f\tcurset = %f\theading = %f\tdiff = %f\n", voltRatio, curSet, filtered, magDiff);
+
+ //reset timer to zero
magn.reset();
}
@@ -571,44 +675,68 @@
//This is proportional feedback only
if(loopTimer.read() > RATE)
{
+ //TODO: put this in a function
+
+ //This moves actuator to the requested position
+ //This is proportional feedback ONLY
+
+ //read analog input from wiper on potentiometer on linear actuator
potVoltage = pot.read();
+ //read analog input from battery voltage
batVoltage = battery.read();
- //voltage = voltage * 3.3f;
+ //calculate ratio of the two (using a ratio prevents battery voltage fluctuation from affecting actual position)
voltRatio = potVoltage / batVoltage;
+ //calculate the absolute value of how far off from requested actuator position we are (saves a few processor cycles)
float absDiff = sqrt((curSet - voltRatio) * (curSet - voltRatio));
+
+ //are we off enough to care? if so, stop moving the actuator
if(absDiff <= RATIO_TOLERANCE)
{
turnStop(1.0f, 1.0f);
- xBee.printf("done\n");
+ //xBee.printf("done\n");
}
+ //do we need to turn right?
else if((curSet - voltRatio) >= 0)
{
if(voltRatio > MAX_RATIO)
{
- xBee.printf("Max Limit Reached\n");
+ //xBee.printf("Max Limit Reached\n");
turnStop(1.0f, 1.0f);
}
else
{
turnRight(1.0f, 1.0f);
- xBee.printf("turning Right\n");
+ //xBee.printf("turning Right\n");
}
}
+ //do we need to turn left?
else
{
if(voltRatio < MIN_RATIO)
{
- xBee.printf("Min Limit Reached\n");
+ //xBee.printf("Min Limit Reached\n");
turnStop(1.0f, 1.0f);
}
else
{
turnLeft(1.0f, 1.0f);
- xBee.printf("turning Left\n");
+ //xBee.printf("turning Left\n");
}
}
- //xBee.printf("battery = %f\tpot = %f\tratio = %f\tmotorSpeed = %f\tmoveSpeed %f\n", (batVoltage * VOLT_MULT), (potVoltage* VOLT_MULT), voltRatio, motorSpeed, moveSpeed);
+
+ //toggle light state
+ green = !green;
+ white = !white;
+
+ xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+
+ //record data to SD card
+ data = fopen("/sd/data.txt", "a");
+ fprintf(data, "%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", wayPtNum, (batVoltage * VOLT_MULT), voltRatio, curSet, filtered, magDiff, polarVector[0], gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+ fclose(data);
+
+ //reset timer to zero
loopTimer.reset();
}
}
@@ -620,7 +748,9 @@
if(mode == 3)
{
+ //stop the trolling motor
goStop();
+
xBee.printf("\nPlease enter position number (0-9), or press y to return to manual mode\n");
while(!xBee.readable());
@@ -634,6 +764,8 @@
}
else
{
+ //TODO: put this in a function
+
xBee.printf("\nFinding most accurate GPS position\nThis will take a few seconds\n\n");
float lowestHDOP = 100;
@@ -654,6 +786,9 @@
}
xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
char tempChar = 'n';
+
+ //first lockup was here
+ //now pressing "1" will break out of while loop
while(!gps.getData() && !(tempChar == '1'))
{
if(xBee.readable())
@@ -667,79 +802,79 @@
{
goalPos[0][0] = curPos[0];
goalPos[0][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '1')
{
goalPos[1][0] = curPos[0];
goalPos[1][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '2')
{
goalPos[2][0] = curPos[0];
goalPos[2][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '3')
{
goalPos[3][0] = curPos[0];
goalPos[3][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '4')
{
goalPos[4][0] = curPos[0];
goalPos[4][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '5')
{
goalPos[5][0] = curPos[0];
goalPos[5][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '6')
{
@@ -750,40 +885,40 @@
{
goalPos[7][0] = curPos[0];
goalPos[7][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '8')
{
goalPos[8][0] = curPos[0];
goalPos[8][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
else if(recChar == '9')
{
goalPos[9][0] = curPos[0];
goalPos[9][1] = curPos[1];
+
//write new coords to SD card
- fr = fopen("/sd/GPS_CORDS.txt", "w+");
-
+ way = fopen("/sd/GPS_CORDS.txt", "w+");
for(int x = 0; x<=9; x++)
{
- fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ fprintf(way, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
}
- fclose(fr);
+ fclose(way);
}
xBee.printf("position %c updated\t", recChar);
}
@@ -830,7 +965,8 @@
arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
//X
arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
-
+
+ //get rid of previous low passed angle data
if(flush)
{
//Use arcTan(x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
@@ -838,6 +974,8 @@
//make negative angles positive
if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
}
+
+ //lowpass filter the angle
else
{
--- a/modSensData.h Mon Apr 27 18:10:52 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,375 +0,0 @@
-/*************************************************************************************************************************************************************/
-// Created by: Ryan Spillman
-//
-// Last updated 4/4/2015
-//
-// Contains several functions such as calculations for roll, pitch, and yaw (tilt compensated compass)
-// Also contains various filtering and calibration functions
-//
-// Requires L3GD20 gyro and LSM303DLHC magnetometer and accelerometer
-/*************************************************************************************************************************************************************/
-
-#include "mbed.h"
-#include "math.h"
-#include "L3GD20.h"
-#include "LSM303DLHC.h"
-
-#define modSensData_h
-
-L3GD20 gyro(PTE25, PTE24);
-LSM303DLHC compass(PTE25, PTE24);
-
-#define ALPHA_H 0.2f //Heading alpha (variable for low pass filter)
-#define ALPHA_A 0.1f //Heading accelerometer (variable for low pass filter)
-#define EARTHRADIUS 6378100.000000000000000000f //Radius of the earth in meters (DO NOT MODIFY)
-#define MAGN_PERIOD (1 / 15.0f) //magnetometer polling period (15 Hz) must manually update hardware ODR registers if chaging this value
-#define ACCEL_PERIOD (1 / 50.0f) //accelerometer polling period (50 Hz) must manually update hardware ODR registers if chaging this value
-#define GYRO_SAMPLE_PERIOD 0.01f //gyro sample period in seconds
-#define M_PI 3.1415926535897932384626433832795f
-#define TWO_PI 6.283185307179586476925286766559f
-#define RADTODEGREE 57.295779513082320876798154814105f
-#define DEGREETORAD 0.01745329251994329576923690768489f
-#define GRAVITY 1.0f
-#define CALIBRATION_COUNT 64
-#define AVG_COUNT 10
-
-// "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
-#define ACCEL_X_MIN -1.000000000f
-#define ACCEL_X_MAX 1.023437500f
-#define ACCEL_Y_MIN -1.015625000f
-#define ACCEL_Y_MAX 1.023437500f
-#define ACCEL_Z_MIN -1.023437500f
-#define ACCEL_Z_MAX 0.960937500f
-
-// "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)
-
-// 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)
-//not sure what the "normal" value is, thus not using scale for magnetometer
-//#define MAGN_X_SCALE (??? / (MAGN_X_MAX - MAGN_X_OFFSET))
-//#define MAGN_Y_SCALE (??? / (MAGN_Y_MAX - MAGN_Y_OFFSET))
-//#define MAGN_Z_SCALE (??? / (MAGN_Z_MAX - MAGN_Z_OFFSET))
-
-
-float magnetom[3] = {0,0,0};
-float accel[3] = {0,0,0};
-float degGyro[3] = {0,0,0};
-float prevDegGyro[3]= {0,0,0};
-float gyroOffset[3] = {0,0,0};
-float roll = 0, pitch = 0, yaw = 0;
-float totalAccel = 0;
-float normalizedGravity;
-
-float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
-
-float prevYaw = 0;
-
-/********************************************************************************************************************************/
-// Low Pass
-/********************************************************************************************************************************/
-
-float lowPass(float input, float output, int select)
-{
- //Magnetometer Heading alpha
- if(select == 0)
- {
- if (output == 0.0000001f) return input;
-
- output = output + ALPHA_H * (input - output);
-
- return output;
- }
- //Accelerometer alpha
- else if(select == 1)
- {
- if (output == 0.0000001f) return input;
-
- output = output + ALPHA_A * (input - output);
-
- return output;
- }
- //GPS heading alpha
- else
- {
- if (output == 0.0000001f) return input;
-
- output = output + ALPHA_A * (input - output);
-
- return output;
- }
-}
-
-/********************************************************************************************************************************/
-// getMagn
-/********************************************************************************************************************************/
-
-void getMagn()
-{
- compass.ReadMagnOnly(&magnetom[0], &magnetom[1], &magnetom[2]);
-
- //Compensate magnetometer error
- //magnetom[0] -= MAGN_X_OFFSET;
- //magnetom[1] -= MAGN_Y_OFFSET;
- //magnetom[2] -= MAGN_Z_OFFSET;
-}
-/********************************************************************************************************************************/
-// getAccel
-/********************************************************************************************************************************/
-
-void getAccel()
-{
- compass.ReadAccOnly(&accel[0], &accel[1], &accel[2]);
-
- //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;
-
-
- for(int i = 0; i <= 3; i++)
- {
- lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
- }
-}
-
-/********************************************************************************************************************************/
-// Integrate Yaw With Gyro Data
-/********************************************************************************************************************************/
-
-void gyroIntegral()
-{
- float yawDiff = yaw - prevYaw;
- float absYawDiff = sqrt(yawDiff * yawDiff);
- //vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
- float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * MAGN_PERIOD);
- float absGyroInt = sqrt(gyroInt * gyroInt);
- prevDegGyro[2] = degGyro[2];
- if(absYawDiff > absGyroInt)
- {
- yaw = prevYaw + gyroInt;
- }
-}
-
-/********************************************************************************************************************************/
-// Compass Heading
-/********************************************************************************************************************************/
-
-void Compass_Heading()
-{
-
- float mag_x;
- float mag_y;
- float cos_roll;
- float sin_roll;
- float cos_pitch;
- float sin_pitch;
-
- //saves a few processor cycles by calculating and storing values
- cos_roll = cos(roll);
- sin_roll = sin(roll);
- cos_pitch = cos(pitch);
- sin_pitch = sin(pitch);
-
- // Tilt compensated magnetic field X
- //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
- mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
- // Tilt compensated magnetic field Y
- //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
- mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
-
- // Magnetic Heading
- yaw = atan2(-mag_x, mag_y);
-
- //make negative angles positive
- if(yaw < 0) yaw = yaw + TWO_PI;
- //yaw = yaw + M_PI;
- yaw = yaw * RADTODEGREE;
-}
-
-/********************************************************************************************************************************/
-// getAttitude
-/********************************************************************************************************************************/
-
-void getAttitude()
-{
- float temp1[3];
- float temp2[3];
- float xAxis[3] = {1.0f, 0.0f, 0.0f};
-
- // GET PITCH
- // Using y-z-plane-component/x-component of gravity vector
- pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
-
- //printf("P = %f", pitch);
-
- // GET ROLL
- // Compensate pitch of gravity vector
- temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
- temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
- temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
-
- temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
- temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
- temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
-
- // Since we compensated for pitch, x-z-plane-component equals z-component:
- roll = atan2(temp2[1], temp2[2]);
-}
-
-/********************************************************************************************************************************/
-// Filtered_Attitude()
-/********************************************************************************************************************************/
-
-void Filtered_Attitude()
-{
- float temp1[3];
- float temp2[3];
- float xAxis[3] = {1.0f, 0.0f, 0.0f};
-
- // GET PITCH
- // Using y-z-plane-component/x-component of gravity vector
- pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
-
- //printf("P = %f", pitch);
-
- // GET ROLL
- // Compensate pitch of gravity vector
- temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
- temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
- temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
-
- temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
- temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
- temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
-
- // Since we compensated for pitch, x-z-plane-component equals z-component:
- roll = atan2(temp2[1], temp2[2]);
-}
-
-
-/********************************************************************************************************************************/
-// updateAngles
-/********************************************************************************************************************************/
-
-void updateAngles()
-//int updateAngles()
-{
- Filtered_Attitude();
- Compass_Heading();
-}
-
-/********************************************************************************************************************************/
-// normalizeGravity
-/********************************************************************************************************************************/
-
-void normalizeGravity()
-{
- float accumulator = 0;
- int sampleCount = 0;
-
-
- //We'll take a certain number of samples and then average them to calculate the average
- while (sampleCount < AVG_COUNT)
- {
- getAccel();
- //add current sample to previous samples
- accumulator += sqrt((accel[0] * accel[0]) + (accel[1] * accel[1]) + (accel[2] * accel[2]));
- sampleCount++;
- //Make sure the IMU has had enough time to take a new sample.
- wait(0.06);
- }
-
-
- //divide by number of samples to get average offset
- normalizedGravity = accumulator / AVG_COUNT;
-
-}
-/********************************************************************************************************************************/
-// gyroAvg
-/********************************************************************************************************************************/
-
-void gyroAvg()
-{
- float accumulator[3] = {0,0,0};
- int sampleCount = 0;
-
-
- //We'll take a certain number of samples and then average them to calculate the offset
- while (sampleCount < AVG_COUNT)
- {
- //get gyro data
- gyro.read(°Gyro[0], °Gyro[1], °Gyro[2]);
- //add current sample to previous samples
- accumulator[2] += degGyro[2];
- sampleCount++;
- //Make sure the gyro has had enough time to take a new sample.
- wait(GYRO_SAMPLE_PERIOD);
- }
-
-
- //divide by number of samples to get average offset
- degGyro[2] = accumulator[2] / AVG_COUNT;
-
-}
-
-/********************************************************************************************************************************/
-// gyroCal
-/********************************************************************************************************************************/
-
-void gyroCal()
-{
- float accumulator[3] = {0,0,0};
- int sampleCount = 0;
-
-
- //We'll take a certain number of samples and then average them to calculate the offset
- while (sampleCount < CALIBRATION_COUNT)
- {
- float gyroData[3];
- //Make sure the gyro has had enough time to take a new sample.
- wait(GYRO_SAMPLE_PERIOD);
- //get gyro data
- gyro.read(&gyroData[0],&gyroData[1],&gyroData[2]);
- for(int i = 0; i < 3; i++)
- {
- //add current sample to previous samples
- accumulator[i] += gyroData[i];
- }
- sampleCount++;
- }
-
- for(int i = 0; i < 3; i++)
- {
- //divide by number of samples to get average offset
- gyroOffset[i] = accumulator[i] / CALIBRATION_COUNT;
- }
-}
-
-/********************************************************************************************************************************/
-// Only Get Gyro Data
-/********************************************************************************************************************************/
-
-void gyroOnly()
-{
- gyro.read(°Gyro[0], °Gyro[1], °Gyro[2]);
- //compensate for gyro offset
- for(int i=0;i<=3;i++)
- {
- degGyro[i] -= gyroOffset[i];
- }
-}
\ No newline at end of file
--- a/navigation.h Mon Apr 27 18:10:52 2015 +0000
+++ b/navigation.h Wed Apr 29 18:07:43 2015 +0000
@@ -17,6 +17,7 @@
// Outputs difference in compass heading(magHead) and heading required(calcHead)
// negative is left and positive is right
/*************************************************************************************************************************************************************/
+//TODO: Check for redundancy
float whichWay(float magHead, float calcHead)
{
float magDiff;