David Spillman
/
GPSNavigationNew
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem
Fork of
GPSNavigation
by 
David Spillman
Revision 5:40ac894e0fa7, committed 2015-04-05
- Comitter:
- Spilly
- Date:
- Sun Apr 05 01:45:29 2015 +0000
- Parent:
- 4:a397b44a0fe8
- Child:
- 6:1341c11ad70c
- Commit message:
- Updated commands
Changed in this revision
--- a/GPS.lib Tue Mar 17 01:13:17 2015 +0000 +++ b/GPS.lib Sun Apr 05 01:45:29 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/Spilly/code/GPS/#1440e1e56a15 +http://developer.mbed.org/users/Spilly/code/GPS2/#af055728e564
--- a/L3GD20.lib Tue Mar 17 01:13:17 2015 +0000 +++ b/L3GD20.lib Sun Apr 05 01:45:29 2015 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/bclaus/code/L3GD20/#62dfce144cf7 +http://developer.mbed.org/users/Spilly/code/L3GD20/#c7a53a17adef
--- a/LSM303DLHC.lib Tue Mar 17 01:13:17 2015 +0000 +++ b/LSM303DLHC.lib Sun Apr 05 01:45:29 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/Spilly/code/LSM303DLHC/#386547b2ee70 +http://developer.mbed.org/users/Spilly/code/LSM303DLHC2/#dd17c7b96e2b
--- a/PID.lib Tue Mar 17 01:13:17 2015 +0000 +++ b/PID.lib Sun Apr 05 01:45:29 2015 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/aberk/code/PID/#6e12a3e5af19 +http://developer.mbed.org/users/Spilly/code/PID/#0861bf4f8f14
--- a/main.cpp Tue Mar 17 01:13:17 2015 +0000
+++ b/main.cpp Sun Apr 05 01:45:29 2015 +0000
@@ -1,292 +1,494 @@
-#include "mbed.h"
-#include "GPS.h"
-#include "modSensData.h"
-#include "move.h"
-#include "PID.h"
+/**************************************************************************************************************************************************************
+// Created by: Ryan Spillman
+//
+// Last updated 4/4/2015
+//
+// This is the software for my teams autonomous boat that is our graduation/final project at Isothermal Community College
+//
+// This build is for testing on a small four wheeled vehicle with skid steering (a motor on each of the four wheels)
+// The user can drive the vehicle by sending chars over the xBee's serial connection
+// After driving to a position, the user can store the GPS position
+// Once postions have been stored, the vehicle will navigate to each of the positions
+// Or, the user can enter static GPS positions in the goal position array and have the vehicle navigate between these positions
+//
+// A PID loop is used to control the heading of the vehicle
+//
+// The project uses a FRDM-K64f (Freescale microcontroller), a LSM303DLHC (magnetometer and accelerometer) to create a tilt compensated comapass,
+// MTK3339 GPS module,xBee Pro S1, and a L298n H-Bridge
+**************************************************************************************************************************************************************/
-//Radius of the earth in meters
-#define EARTHRADIUS 6378100.0f
-//Tolerance for heading actual and heading needed
-#define HEADDIFF 0.0f
-//Tolerance for whether or not vehicle has arrived at goal position
-#define ARRIVED 100.0f
-//Period in seconds of the the main loop
-#define PERIOD 0.5f
-//Period in seconds of PID loop
-#define RATE 0.05f
-#define PIDCYCLES 100
-//GPS
-GPS gps(PTC4, PTC3);
+#include "mbed.h"
+#include "PID.h"
+#include "modSensData.h"
+#include "navigation.h"
+#include "GPS.h"
-//X-Bee
-Serial xBee(PTD3, PTD2); //boat
-//Serial xBee(PTC15, PTC14); //car
+#define EARTHRADIUS 6378100.000000000000000000f //Radius of the earth in meters
+#define ARRIVED 2.0f //Tolerance for whether or not vehicle has arrived at goal position
+#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 GPS_PERIOD (1 / 5.0f) //GPS polling period (5 Hz) must manually update hardware ODR registers if chaging this value
+#define GPS_ALPHA 0.3f
+#define DELAYTIME 0.1f //delay time between recieved chars for increasing motor speed
+#define userKc 1.8f //directly proportional
+#define userTi 7.7f //a larger number makes the integral have less affect on the output
+#define userTd 0.0f //a smaller number makes the derivative have less affect on the output
-//PID
-//Kc, Ti, Td, interval
-PID controller(.1, .005, .01, RATE);
+Serial xBee(PTB11, PTB10); //UART 3
+GPS gps(PTC15, PTC14); //UART 4
+
+PID headingPID(userKc, userTi, userTd, MAGN_PERIOD); //Kc, Ti, Td, interval
Timer t2;
+Timer headingTime;
+Timer acc;
+Timer magn;
+Timer inputTimer;
-//Enter new position here
-float goalPos[2] = {35.336020, -81.912420};
-
-float startPos[2], curPos[2], polarVector[2];
+//Prototype functions
+void updateMotorSpeed(char tempChar);
+void updateDir(char tempChar);
+void setGoalPos(float lat, float lon);
+void makeVector(float posZero, float posOne, float curPos[2]);
+//End of prototype functions
-int sCheck, pCheck = 1;
+//Enter new positions here
+float goalPos[10][2] = { {35.478407, -81.981978},
+ {35.478344, -81.981986},
+ {35.478407, -81.981978},
+ {35.478344, -81.981986},
+ {35.478407, -81.981978},
+ {35.478407, -81.981978},
+ {35.478344, -81.981986},
+ {35.478407, -81.981978},
+ {35.478344, -81.981986},
+ {35.478407, -81.981978}
+ };
-void setGoalPos(float lat, float lon);
-void makeVector(void);
-float whichWay(float magHead, float calcHead);
-void testYaw(void);
+float polarVector[2] = {0,0};
+float motorSpeed = 0.5f;
+float curPos[2] = {0,0};
int main()
{
- xBee.baud(9600);
+ int wayPtNum = 0, mode = 0;
+ char prevChar = 'z';
+ float magDiff = 0;
+ xBee.baud(115200);
xBee.printf("\nI'm Alive...\n");
+ xBee.printf("Press any key to begin\n");
+
+ //wait for keypress
+ while(!xBee.readable());
+
+ //initialize magnetometer, accelerometer
+ compass.init();
//Setup the GPS
gps.Init();
+
xBee.printf("gps initialized\n");
- while(!xBee.readable())
- {
- xBee.printf("Press any key to begin\n");
- wait(1);
- }
- 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");
+ t2.start();
//wait until we have a gps fix
while(gps.fixtype == 0)
{
- xBee.printf("fix %d\n", gps.fixtype);
- gps.parseData();
- wait(.2);
+ if(t2.read() >= GPS_PERIOD)
+ {
+ //xBee.printf("fix %d\n", gps.fixtype);
+ gps.parseData();
+ xBee.printf("fix %d\n", gps.fixtype);
+ getAccel();
+
+ //xBee.printf("fix %d\n", gps.fixtype);
+ xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+ //wait(GPSPERIOD);
+ t2.reset();
+ }
}
+ //put GPS in sleep mode to prevent overflowing the buffer
+ gps.Sleep(1);
- //set starting position
+ //get IMU data and calculate the tilt compensated compass
+ getAccel();
+ getMagn();
+ updateAngles();
+
+ //set current position
curPos[0] = gps.latitude;
curPos[1] = gps.longitude;
-
- //Convert starting position and goal position to a vector
- makeVector();
+ makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
- //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
- //printf("magn %f\tangle %f\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
+ xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+ xBee.printf("dist %f\theading %f\n", polarVector[0], polarVector[1]);
+ xBee.printf("starting main loop\n");
+
+ //PID control of left and right motors based on input from gyro
+ //Goal is to have the vehicle go straight
+ headingPID.setInputLimits(-180, 180);
+ headingPID.setOutputLimits(-0.5f, 0.5f);
+ //set mode to auto
+ headingPID.setMode(0);
+ //We want the difference to be zero
+ headingPID.setSetPoint(0);
+ headingPID.setBias(0.1f);
-
- xBee.printf("starting main loop\n");
+ float filtered = 0.0000001f;
+ int gpsCount = 0;
+ headingTime.start();
+ acc.start();
+ magn.start();
+ xBee.printf("Starting Motors!\n");
+ goForward(0.5f, 0.5f);
+ int wakeUp = 1;
while (1)
{
- //Emergency stop
- if(xBee.readable())
+ //manual mode
+ if(mode == 0)
{
- char tempChar = xBee.getc();
- if(tempChar == 'n')
+ if(t2.read() >= GPS_PERIOD)
+ {
+ //check GPS data
+ gps.parseData();
+ curPos[0] = curPos[0] + GPS_ALPHA * (gps.latitude - curPos[0]);
+ curPos[1] = curPos[1] + GPS_ALPHA * (gps.longitude - curPos[1]);
+ xBee.printf("Current Postion:\t%f\t%f\n", curPos[0], curPos[1]);
+
+ t2.reset();
+ }
+ //checks to see if data is available on xBee
+ if(xBee.readable())
{
- xBee.printf("emergency stop\n");
- goStop(1,1);
- while(1);
+ char recChar = xBee.getc();
+
+ //change to autonomous mode
+ if(recChar == 'z')
+ {
+ mode = 1;
+ }
+ //change to record mode
+ if(recChar == 'r')
+ {
+ mode = 3;
+ }
+ //did we recieve a new char?
+ else if(recChar != prevChar)
+ {
+ if(recChar != 'p' && recChar != 'm' && recChar != '1')
+ {
+ updateDir(recChar);
+ }
+
+ else updateMotorSpeed(recChar);
+
+ //reset timer
+ inputTimer.reset();
+ inputTimer.start();
+
+ //keep recieved char to compare to next recieved char
+ prevChar = recChar;
+
+ }
+
+ else if(recChar != 'p' && recChar != 'm' && recChar != '1')
+ {
+ updateDir(recChar);
+ }
+
+ //if the char is the same as the previously recieved char,
+ //has the delay time elapsed since the first same char
+ else if(inputTimer.read_ms() >= DELAYTIME)
+ {
+ updateMotorSpeed(recChar);
+ }
}
}
- //check GPS data
- //xBee.printf("GPS parse data attemp = %d\n", gps.parseData());
- gps.parseData();
- //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
- //update current position
- curPos[0] = gps.latitude;
- curPos[1] = gps.longitude;
-
- //makeVector();
- //xBee.printf("Main 3\n");
- //get data from IMU and do calculations to determine heading
- //updateAngles();
- //updateAngles();
-
- float latDif = sqrt((goalPos[0] - curPos[0]) * (goalPos[0] - curPos[0]));
- float longDif = sqrt((goalPos[1] - curPos[1]) * (goalPos[1] - curPos[1]));
-
- //Get absolute value of how far off goal latitude and longitude are from current latitude and longitude
- //If it is less than tolerance for arriving, stop
- if(polarVector[0] <= ARRIVED)
- {
- xBee.printf("We Have Arrived\n");
- goStop(1,1);
- sCheck = 0;
- while(1);
- }
-
- if(updateAngles())
+ //autonomous mode
+ if(mode == 1)
{
- makeVector();
- float magDiff = whichWay(yaw, polarVector[1]);
- controller.setProcessValue(magDiff);
-
- motorSpeed = controller.compute();
- goForward(0.5f + motorSpeed,0.5f - motorSpeed);
- wait(RATE);
- }
- //xBee.printf("dist %f\tneed %f\tcurrent %f\tdir %c\tlat %f\tlon %f\tgoalLat %f\tgoalLon %f\n", polarVector[0], polarVector[1], yaw, direction, gps.latitude, gps.longitude, goalPos[0], goalPos[1]);
- }
-}
-
-//create polar vector based on two sets of latitude and longitude
-void makeVector(void)
-{
- float arcLength[2];
-
- //arc length = radius * angle
- //Y
- arcLength[1] = EARTHRADIUS * (goalPos[0] - curPos[0]);
- //X
- arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
-
- //calculate magnitude of vector
- polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
-
- //Use arcTan(-x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
- polarVector[1] = (RADTODEGREE * (atan2(-arcLength[0], arcLength[1])));
-
- //make negative angles positive
- if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
-}
-
-//Outputs difference in compass heading(magHead) and heading required(calcHead)
-//negative is left and positive is right
-float whichWay(float magHead, float calcHead)
-{
- float magDiff;
-
- float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
-
- //Is the heading off enough to care?
- if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
- //if(1)
- {
- //quadrant I
- if(calcHead < 90)
- {
- if(calcHead < magHead)
+ //only send wake up once
+ if(wakeUp == 1)
+ {
+ //wake up GPS
+ gps.Sleep(0);
+ wakeUp = 0;
+ }
+ //Emergency stop
+ if(xBee.readable())
{
- if(magHead < (180 + calcHead))
+ char recChar = xBee.getc();
+ if(recChar == 'n')
{
- //turn left need negative
- magDiff = calcHead - magHead;
+ xBee.printf("emergency stop\n");
+ goStop(1,1);
+ while(1);
}
- else
+ //change to manual mode
+ if(recChar == 'y')
{
- //turn right need positive
- magDiff = calcHead - magHead + 360;
+ mode = 0;
}
}
else
{
- if(magHead < (180 + calcHead))
- {
- //turn left need negative
- magDiff = calcHead - magHead;
- }
- else
+ if(t2.read() >= GPS_PERIOD)
{
- //turn right need positive
- magDiff = calcHead - magHead + 360;
+ //check GPS data
+ gps.parseData();
+ //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+ curPos[0] = curPos[0] + GPS_ALPHA * (gps.latitude - curPos[0]);
+ curPos[1] = curPos[1] + GPS_ALPHA * (gps.longitude - curPos[1]);
+ t2.reset();
+
+ gpsCount = gpsCount + 1;
+
+ //low pass filter 5 gps data sets before recalculating heading and reseting PID data (should make for less oscillation)
+ if(gpsCount == 5)
+ {
+ makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
+ //latDif = sqrt((goalPos[wayPtNum][0] - curPos[0]) * (goalPos[wayPtNum][0] - curPos[0]));
+ //longDif = sqrt((goalPos[wayPtNum][1] - curPos[1]) * (goalPos[wayPtNum][1] - curPos[1]));
+ //xBee.printf("\n\nupdated vector\n\n");
+
+ headingPID.reset(); //Reset PID data since we have a new heading
+ gpsCount = 0;
+ }
+ xBee.printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n", polarVector[0], magDiff, curPos[0], curPos[1], goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
+ //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], yaw);
+ t2.reset();
}
- }
- }
- //quadrant II
- else if(calcHead < 180)
- {
- if(calcHead < magHead)
- {
- if(magHead < (180 + calcHead))
+
+ if(acc.read() >= ACCEL_PERIOD)
+ {
+ getAccel();
+ acc.reset();
+ }
+ if(magn.read() >= MAGN_PERIOD)
{
- //turn left need negative
- magDiff = calcHead - magHead;
- }
- else
- {
- //turn right need positive
- magDiff = calcHead - magHead + 360;
+ getMagn();
+ updateAngles();
+ magn.reset();
+ //compassAvg();
+
+ filtered = lowPass(yaw, filtered, 0);
+ //xBee.printf("yaw = %f\tfiltered = %f\n", yaw, filtered);
+ //set heading to a fixed number for testing
+ magDiff = whichWay(filtered, polarVector[1]);
+ //magDiff = whichWay(yaw, polarVector[1]);
+ //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], yaw);
+ //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], filtered);
+ headingPID.setProcessValue(magDiff);
+
+ motorSpeed = headingPID.compute();
+ goForward(0.5f - motorSpeed,0.5f + motorSpeed);
+
+ //If it is less than tolerance for arriving, stop
+ if(polarVector[0] <= ARRIVED)
+ {
+ xBee.printf("Goal Position %d reached!\n", wayPtNum);
+ wait(1);
+ wayPtNum ++;
+ if(wayPtNum >= 6)
+ {
+ xBee.printf("Final Position Reached!\nShutting down\n");
+ goStop(1,1);
+ while(1);
+ }
+ xBee.printf("Moving to Goal Position %d\n", wayPtNum);
+ }
}
- }
- else
- {
- if(magHead < (180 + calcHead))
- {
- //turn left need negative
- magDiff = calcHead - magHead;
- }
- else
- {
- //turn right need positive
- magDiff = calcHead - magHead + 360;
- }
+
+ //xBee.printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n\n", polarVector[0], magDiff, gps.latitude, gps.longitude, goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
+ //printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n\n", polarVector[0], magDiff, gps.latitude, gps.longitude, goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
+
+ //wait for elapsed time to be greater than the period of the heading loop
+ //while(headingTime.read() < RATE);
+ //headingTime.reset();
}
}
- //quadrant III
- else if(calcHead < 270)
+ //record position
+ if(mode == 3)
{
- if(calcHead < magHead)
+ xBee.printf("Please enter position number (0-9), or press y to return to manual mode\nenter selection again if confirmation does not appear\n");
+
+ while(!xBee.readable());
+
+ char recChar = xBee.getc();
+ recChar = xBee.getc();
+ //return to manual mode
+ if(recChar == 'y')
{
- if(magHead < (180 + calcHead))
- {
- //turn left need negative
- magDiff = calcHead - magHead;
- }
- else
- {
- //turn right need positive
- magDiff = calcHead - magHead + 360;
- }
+ mode = 0;
}
- else
+ else
{
- if(magHead < (180 + calcHead))
+ if(recChar == '0')
+ {
+ goalPos[0][0] = curPos[0];
+ goalPos[0][1] = curPos[1];
+ }
+ else if(recChar == '1')
+ {
+ goalPos[1][0] = curPos[0];
+ goalPos[1][1] = curPos[1];
+ }
+ else if(recChar == '2')
+ {
+ goalPos[2][0] = curPos[0];
+ goalPos[2][1] = curPos[1];
+ }
+ else if(recChar == '3')
{
- //turn left need negative
- magDiff = calcHead - magHead;
+ goalPos[3][0] = curPos[0];
+ goalPos[3][1] = curPos[1];
+ }
+ else if(recChar == '4')
+ {
+ goalPos[4][0] = curPos[0];
+ goalPos[4][1] = curPos[1];
+ }
+ else if(recChar == '5')
+ {
+ goalPos[5][0] = curPos[0];
+ goalPos[5][1] = curPos[1];
+ }
+ else if(recChar == '6')
+ {
+ goalPos[6][0] = curPos[0];
+ goalPos[6][1] = curPos[1];
+ }
+ else if(recChar == '7')
+ {
+ goalPos[7][0] = curPos[0];
+ goalPos[7][1] = curPos[1];
}
- else
+ else if(recChar == '8')
+ {
+ goalPos[8][0] = curPos[0];
+ goalPos[8][1] = curPos[1];
+ }
+ else if(recChar == '9')
{
- //turn right need positive
- magDiff = calcHead - magHead + 360;
+ goalPos[9][0] = curPos[0];
+ goalPos[9][1] = curPos[1];
}
+ xBee.printf("position %c updated\n", recChar);
}
+ xBee.printf("exiting record mode\n");
+ mode = 0;
+ }
+ }
+}
+
+void updateMotorSpeed(char tempChar)
+{
+ if(tempChar == '1')
+ {
+ goStop(1, 1);
+ xBee.printf("Stop\n%.2f\t", motorSpeed);
}
- //quadrant IV
+
+ else if(tempChar == 'p')
+ {
+ if(motorSpeed < 1)
+ {
+ motorSpeed = motorSpeed + 0.01f;
+ xBee.printf("Increasing MotorSpeed\n%.2f\t", motorSpeed);
+ }
+ else(xBee.printf("Maximum motorSpeed reached\n%.2f\t", motorSpeed));
+
+ //errors with floating point calculations are causing motorSpeed to exceed 1
+ //this is a workaround
+ if(motorSpeed > 1)
+ {
+ motorSpeed = 1;
+ }
+ }
+
+ else if(tempChar == 'm')
+ {
+ if(motorSpeed > 0.01f)
+ {
+ motorSpeed = motorSpeed - 0.01f;
+ xBee.printf("MotorSpeed decreased\n%.2f\t", motorSpeed);
+ }
else
{
-
- if(calcHead < magHead)
- {
- magDiff = calcHead - magHead;
- }
- else
- {
- if(magHead < (calcHead - 180))
- {
-
- magDiff = calcHead - 360 - magHead;
- }
- else
- {
- //turn right need positive
- magDiff = calcHead - magHead;
- }
- }
+ //calculation errors may prevent motorSpeed from reaching actual zero
+ motorSpeed = 0;
+ xBee.printf("Minimum motorSpeed reached\n%.2f\t", motorSpeed);
+ }
+ }
+}
+
+void updateDir(char tempChar)
+{
+ if(tempChar == 'w')
+ {
+ goForward(motorSpeed, motorSpeed);
+ //wait(time);
+ //goStop(1,1);
+ xBee.printf("Going forward\n%.2f\t", motorSpeed);
+ }
+ else if(tempChar == 's')
+ {
+ goBackward(motorSpeed, motorSpeed);
+ //wait(time);
+ //goStop(1,1);
+ xBee.printf("Going backward\n%.2f\t", motorSpeed);
+ }
+ else if(tempChar == 'd')
+ {
+ goRight(motorSpeed, motorSpeed);
+ //wait(time);
+ //goStop(1,1);
+ xBee.printf("Going right\n%.2f\t", motorSpeed);
+ }
+ else if(tempChar == 'a')
+ {
+ goLeft(motorSpeed, motorSpeed);
+ //wait(time);
+ //goStop(1,1);
+ xBee.printf("Going left\n%.2f\t", motorSpeed);
}
- return magDiff;
- }
- return 0;
-}
\ No newline at end of file
+}
+
+
+//create polar vector based on two sets of latitude and longitude
+void makeVector(float posZero, float posOne, float curPos[2])
+{
+ float arcLength[2];
+ float goalPos[2];
+ goalPos[0] = posZero;
+ goalPos[1] = posOne;
+
+ //arc length = radius * angle
+ //Y
+ //arcLength[1] = EARTHRADIUS * (sqrt((goalPos[0] * goalPos[0])) - sqrt((curPos[0] * curPos[0])));
+ //arcLength[1] = EARTHRADIUS * (sqrt((goalPos[0] - curPos[0]) * (goalPos[0] - curPos[0])));
+ arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
+ //xBee.printf("%f\n", arcLength[1]);
+ //X
+ //arcLength[0] = EARTHRADIUS * (sqrt((goalPos[1] - curPos[1]) * (goalPos[1] - curPos[1])));
+ arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
+ //printf("G0 = %f\t G1 = %f\tC0 = %f\tC1 = %f\tA0 = %f\tA1 = %f\n", goalPos[0], goalPos[1], curPos[0], curPos[1], arcLength[0], arcLength[1]);
+ //xBee.printf("%f\n", goalPos[1]);
+ //xBee.printf("%f\n", curPos[1]);
+
+ //xBee.printf("%f\n", arcLength[0]);
+ //calculate magnitude of vector
+ polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
+ //xBee.printf("%f\n", polarVector[0]);
+
+ //Use arcTan(-x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
+ polarVector[1] = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
+
+ //printf("0 = %f\t1 = %f\theading = %f\n", arcLength[0], arcLength[1], polarVector[1]);
+
+ //make negative angles positive
+ if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
+ //xBee.printf("%f\n", polarVector[1]);
+}
--- a/modSensData.h Tue Mar 17 01:13:17 2015 +0000
+++ b/modSensData.h Sun Apr 05 01:45:29 2015 +0000
@@ -1,32 +1,62 @@
+/**************************************************************************************************************************************************************
+// 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 M_PI 3.1415926535897932384626433832795f
-#define TWO_PI 6.283185307179586476925286766559f
-#define RADTODEGREE 57.295779513082320876798154814105f
-#define GRAVITY 1.0f
-#define THRESHOLD 1.1f
+//Variable for low pass filter
+#define ALPHA_H 0.2f
+#define ALPHA_A 0.1f
+#define MAX_DELTA_HEADING 20
+
+#define M_PI 3.1415926535897932384626433832795f
+#define TWO_PI 6.283185307179586476925286766559f
+#define RADTODEGREE 57.295779513082320876798154814105f
+#define DEGREETORAD 0.01745329251994329576923690768489f
+#define GRAVITY 1.0f
+#define THRESHOLD 0.02f
+
+//gyro sample period in seconds
+#define GYRO_SAMPLE_PERIOD 0.01f
+#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 ((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 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)
+#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)
+
+
+//Period in seconds of heading PID loop
+#define COMPASS_PERIOD 0.060f
+
+#define YAW_VAR_TOLER 5.0f
// Sensor calibration scale and offset values
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
@@ -39,80 +69,150 @@
#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))
-
+//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))
-//calibrated reading = (reading - offset) * scale
-
-Timer t1;
+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 vC[3], cC[3], magnetom[3], inertialAccel[3];
+float filteredYaw, filteredPitch, filteredRoll;
-float aP[3], vP[3], cP[3], deltaT, timeStamp, degGyro[3], angPos[3], prevAngPos[3];
+float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
+float lowPassMagnetom[3] = {0.0000001f,0.0000001f,0.0000001f};
-double roll = 0, pitch = 0, yaw = 0;
-
-float magCheck;
-
-float accel[3], bias[3], magnDiff;
-
-int printStamp = 0, stopStamp = 0, dangerAhead = 0, howLong = 0, count[3], pos[5][3]= { {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0} };
-
-int flag = 0, signBit = 0;
+float prevYaw = 0;
+int cycleCount = 0;
+int firstPass = 1;
+/*
+float lowPass(float input, float output)
+{
+ if (output == 0.0000001f) return input;
+
+ output = (ALPHA * input) + ((1 - ALPHA) * output);
+
+ return output;
+}
+*/
+float lowPass(float input, float output, int select)
+{
+ //Accelerometer alpha
+ if(select == 1)
+ {
+ if (output == 0.0000001f) return input;
+
+ output = output + ALPHA_A * (input - output);
+
+ return output;
+ }
+ //Heading alpha
+ else
+ {
+ if (output == 0.0000001f) return input;
+
+ output = output + ALPHA_H * (input - output);
+
+ return output;
+ }
+}
/********************************************************************************************************************************/
-// Get Accelerometer and Magnetometer Data
+// 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 readIMU()
+void getAccel()
{
- 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;
+ 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;
+ 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]);
+ for(int i = 0; i <= 3; i++)
+ {
+ lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
+ }
}
-
+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)) * COMPASS_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;
- // 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 * RADTODEGREE;
+
+ float mag_x;
+ float mag_y;
+ //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
+ //{
+ 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;
+ //}
+ /*
+ else
+ {
+ mag_x = magnetom[0] ;
+ mag_y = magnetom[1];
+ }
+ */
+ // 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;
}
/********************************************************************************************************************************/
@@ -146,68 +246,113 @@
}
/********************************************************************************************************************************/
-// rotateBTI
+// Compass Heading
/********************************************************************************************************************************/
-//rotation from the body frame to the inertial frame
-void rotateBTI()
+void Filtered_Compass_Heading()
{
- float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
-
- float gravity[3] = {0, 0, -GRAVITY};
-
- float rIB[3][3];
-
- //saves a few processor cycles by calculating and storing values
-
- //Serial.print("Roll");
- cos_roll = cos(roll);
- //Serial.print(cos_roll, 4);
- //Serial.print(" ");
- sin_roll = sin(roll);
- //Serial.print(sin_roll, 4);
- //Serial.print(" ");
+
+ float mag_x;
+ float mag_y;
+ //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
+ //{
+ 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(filteredRoll);
+ sin_roll = sin(filteredRoll);
+ cos_pitch = cos(filteredPitch);
+ sin_pitch = sin(filteredPitch);
+
+ // 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 = lowPassMagnetom[0] * cos_pitch + lowPassMagnetom[1] * sin_roll * sin_pitch + lowPassMagnetom[2] * cos_roll * sin_pitch;
+ // Tilt compensated magnetic field Y
+ //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+ mag_y = lowPassMagnetom[1] * cos_roll - lowPassMagnetom[2] * sin_roll;
+ //}
+ /*
+ else
+ {
+ mag_x = magnetom[0] ;
+ mag_y = magnetom[1];
+ }
+ */
+ // Magnetic Heading
+ filteredYaw = atan2(-mag_x, mag_y);
+
+
+ //make negative angles positive
+ if(filteredYaw < 0) filteredYaw = filteredYaw + TWO_PI;
+ //yaw = yaw + M_PI;
+ filteredYaw = filteredYaw * RADTODEGREE;
+}
+
+/********************************************************************************************************************************/
+// Filtered_Attitude()
+/********************************************************************************************************************************/
+
+void Filtered_Attitude()
+{
+ float temp1[3];
+ float temp2[3];
+ float xAxis[3] = {1.0f, 0.0f, 0.0f};
- //Serial.print("Pitch");
- cos_pitch = cos(pitch);
- //Serial.print(cos_pitch, 4);
- //Serial.print(" ");
- sin_pitch = sin(pitch);
- //Serial.print(sin_pitch, 4);
- //Serial.print(" ");
+ // 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])));
- //Serial.print("YAW");
- cos_yaw = cos(yaw);
- //Serial.print(cos_yaw, 4);
- //Serial.print(" ");
- sin_yaw = sin(yaw);
- //Serial.print(sin_yaw, 4);
- //Serial.print(" ");
+ //printf("P = %f", pitch);
- rIB[0][0] = cos_yaw * cos_pitch;
- rIB[0][1] = (cos_yaw * sin_roll * sin_pitch) - (cos_roll * sin_yaw);
- rIB[0][2] = (sin_roll * sin_yaw) + (cos_roll * cos_yaw * sin_pitch);
- rIB[1][0] = cos_pitch * sin_yaw;
- rIB[1][1] = (cos_roll * cos_yaw) + (sin_roll * sin_yaw * sin_pitch);
- rIB[1][2] = (cos_roll * sin_yaw * sin_pitch) - (cos_yaw * sin_roll);
- rIB[2][0] = -1 * sin_pitch;
- rIB[2][1] = cos_pitch * sin_roll;
- rIB[2][2] = cos_roll * cos_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]);
- for(int i = 0; i < 3; i++)
- {
- aP[i] = inertialAccel[i];
- inertialAccel[i] = rIB[i][0] * accel[0] + rIB[i][1] * accel[1] + rIB[i][2] * accel[2];
- inertialAccel[i] = inertialAccel[i] + gravity[i];
- }
+ 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
+/********************************************************************************************************************************/
-int updateAngles()
+void updateAngles()
+//int updateAngles()
{
- readIMU();
- if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
+ //readIMU();
+ //getAttitude();
+ Filtered_Attitude();
+ Compass_Heading();
+ //Filtered_Attitude();
+ //Filtered_Compass_Heading();
+ /*
+ totalAccel = sqrt((lowPassAccel[0] * lowPassAccel[0]) + (lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2]));
+
+ prevYaw = yaw;
+ getAttitude();
+ Compass_Heading();
+
+ if((sqrt((prevYaw - yaw) * (prevYaw - yaw)) >= YAW_VAR_TOLER) && (firstPass != 1))
+ {
+ yaw = prevYaw;
+ }
+ else if(firstPass == 1)
+ {
+ firstPass = 0;
+ }
+
+ //if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
+ if((totalAccel <= (normalizedGravity + THRESHOLD)) && (totalAccel >= (normalizedGravity - THRESHOLD)))
{
//float prevYaw = yaw;
getAttitude();
@@ -216,18 +361,193 @@
//check for noise
//if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
//{
- return 1;
+ return 1;
//}
//return 0;
}
- return 0;
- //rotateBTI();
+ */
+ //return 0;
+ //else Compass_Heading();
+ //else return 0;
+ //getAttitude();
+ //Compass_Heading();
+ //gyroIntegral();
+}
+
+/********************************************************************************************************************************/
+// magAvg
+/********************************************************************************************************************************/
+
+void compassAvg()
+{
+ float accumulator = 0;
+ //float prevYaw[AVG_COUNT];
+ int sampleCount = 0;
+
+
+ //We'll take a certain number of samples and then average them to calculate the offset
+ while (sampleCount < AVG_COUNT)
+ {
+ updateAngles();
+
+ //add current sample to previous samples
+ accumulator += yaw;
+
+ sampleCount++;
+ //Make sure the gyro has had enough time to take a new sample.
+ wait(COMPASS_PERIOD);
+ }
+
+ //divide by number of samples to get average offset
+ yaw = accumulator / AVG_COUNT;
+
+
+}
+
+/********************************************************************************************************************************/
+// magLowPass
+/********************************************************************************************************************************/
+void compLowPass()
+{
+ /*
+ prevMagnetom[5][3];
+ //read sensors five times
+ for(int x = 0; x<= 5; i++)
+ {
+ for(int i = 0; i<=3; i++)
+ {
+ prevMagnetom[x, i] = magnetom[i];
+ }
+ readIMU();
+ }
+ float diff[5][3];
+ for(int x = 1; x<= 5; i++)
+ {
+ sqrt(prevMagnetom[x][0]
+ for(int i = 0; i<=3; i++)
+ {
+ if(diff[x][i] = sqrt((prevMagnetom[x][i] - prevMagnemtom[x - 1]) * (prevMagnetom[x][i] - prevMagnemtom[x - 1])) >= MAG_VAR_TOLER)
+ {
+
+ }
+ }
+ }
+
+ float deltaHeading = 0;
+ updateAngles();
+ deltaHeading = sqrt((yaw - prevYaw[cycleCount]) * (yaw - prevYaw[cycleCount]));
+
+ if(deltaHeading >= MAX_DELTA_HEADING)
+ {
+ yaw = (yaw + prevYaw[cycleCount]) / 2.0f;
+ }
+ cycleCount = cycleCount + 1;
+ prevYaw[cycleCount] = yaw;
+
+ if(cycleCount >= 4)
+ {
+ cycleCount = 0;
+ prevYaw = {0,0,0,0,0}
+ }
+*/
}
-float getGyro()
+
+/********************************************************************************************************************************/
+// 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 gx, gy, gz;
- gyro.read(&gx,&gy,&gz);
- return gz;
-}
\ No newline at end of file
+ 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;
+ }
+}
+
+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];
+ }
+}
+
--- a/move.h Tue Mar 17 01:13:17 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,57 +0,0 @@
-//L298n connections
-DigitalOut pinI1(D5);
-DigitalOut pinI2(D6);
-DigitalOut pinI3(D10);
-DigitalOut pinI4(D11);
-PwmOut ENA(D12); //Left
-PwmOut ENB(D13); //Right
-
-void goStop(float valueOne, float valueTwo)
-{
- pinI1 = 0;
- pinI2 = 0;
- pinI3 = 0;
- pinI4 = 0;
- ENA = valueOne;
- ENB = valueTwo;
-}
-
-void goForward(float valueOne, float valueTwo)
-{
- pinI1 = 1;
- pinI2 = 0;
- pinI3 = 0;
- pinI4 = 1;
- ENA = valueOne;
- ENB = valueTwo;
-}
-
-void goBackward(float valueOne, float valueTwo)
-{
- pinI1 = 0;
- pinI2 = 1;
- pinI3 = 1;
- pinI4 = 0;
- ENA = valueOne;
- ENB = valueTwo;
-}
-
-void goLeft(float valueOne, float valueTwo)
-{
- pinI1 = 0;
- pinI2 = 1;
- pinI3 = 0;
- pinI4 = 1;
- ENA = valueOne;
- ENB = valueTwo;
-}
-
-void goRight(float valueOne, float valueTwo)
-{
- pinI1 = 1;
- pinI2 = 0;
- pinI3 = 1;
- pinI4 = 0;
- ENA = valueOne;
- ENB = valueTwo;
-}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/navigation.h Sun Apr 05 01:45:29 2015 +0000
@@ -0,0 +1,204 @@
+/**************************************************************************************************************************************************************
+// Created by: Ryan Spillman
+//
+// Last updated 4/4/2015
+//
+// Contains function that outputs difference in compass heading(magHead) and heading required(calcHead)
+// Also contains functions for controlling L298n H-bridge
+**************************************************************************************************************************************************************/
+
+#define navigation_h
+
+//Tolerance for heading actual and heading needed
+#define HEADDIFF 0.000000000000000000f
+
+//L298n connections
+DigitalOut pinI1(D10);
+DigitalOut pinI2(D11);
+DigitalOut pinI3(D12);
+DigitalOut pinI4(D13);
+PwmOut ENA(D6); //Left
+PwmOut ENB(D7); //Right
+
+//Outputs difference in compass heading(magHead) and heading required(calcHead)
+//negative is left and positive is right
+float whichWay(float magHead, float calcHead)
+{
+ float magDiff;
+
+ float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
+
+ //Is the heading off enough to care?
+ if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
+ //if(1)
+ {
+ //quadrant I
+ if(calcHead < 90)
+ {
+ if(calcHead < magHead)
+ {
+ if(magHead < (180 + calcHead))
+ {
+ //turn left need negative
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+ //turn right need positive
+ magDiff = calcHead - magHead + 360;
+ }
+ }
+ else
+ {
+ if(magHead < (180 + calcHead))
+ {
+ //turn left need negative
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+ //turn right need positive
+ magDiff = calcHead - magHead + 360;
+ }
+ }
+ }
+ //quadrant II
+ else if(calcHead < 180)
+ {
+ if(calcHead < magHead)
+ {
+ if(magHead < (180 + calcHead))
+ {
+ //turn left need negative
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+ //turn right need positive
+ magDiff = calcHead - magHead + 360;
+ }
+ }
+ else
+ {
+ if(magHead < (180 + calcHead))
+ {
+ //turn left need negative
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+ //turn right need positive
+ magDiff = calcHead - magHead + 360;
+ }
+ }
+ }
+ //quadrant III
+ else if(calcHead < 270)
+ {
+ if(calcHead < magHead)
+ {
+ if(magHead < (180 + calcHead))
+ {
+ //turn left need negative
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+ //turn right need positive
+ magDiff = calcHead - magHead + 360;
+ }
+ }
+ else
+ {
+ /*
+ if(magHead < (180 + calcHead))
+ {
+ //turn left need negative
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+
+ //turn right need positive
+ magDiff = calcHead - magHead + 360;
+ }
+ */
+ magDiff = calcHead - magHead - 90;
+ }
+ }
+ //quadrant IV
+ else
+ {
+
+ if(calcHead < magHead)
+ {
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+ if(magHead < (calcHead - 180))
+ {
+
+ magDiff = calcHead - 360 - magHead;
+ }
+ else
+ {
+ //turn right need positive
+ magDiff = calcHead - magHead;
+ }
+ }
+ }
+ return magDiff;
+ }
+ return 0;
+}
+
+void goStop(float valueOne, float valueTwo)
+{
+ pinI1 = 0;
+ pinI2 = 0;
+ pinI3 = 0;
+ pinI4 = 0;
+ ENA = valueOne;
+ ENB = valueTwo;
+}
+
+void goForward(float valueOne, float valueTwo)
+{
+ pinI1 = 1;
+ pinI2 = 0;
+ pinI3 = 0;
+ pinI4 = 1;
+ ENA = valueOne;
+ ENB = valueTwo;
+}
+
+void goBackward(float valueOne, float valueTwo)
+{
+ pinI1 = 0;
+ pinI2 = 1;
+ pinI3 = 1;
+ pinI4 = 0;
+ ENA = valueOne;
+ ENB = valueTwo;
+}
+
+void goLeft(float valueOne, float valueTwo)
+{
+ pinI1 = 0;
+ pinI2 = 1;
+ pinI3 = 0;
+ pinI4 = 1;
+ ENA = valueOne;
+ ENB = valueTwo;
+}
+
+void goRight(float valueOne, float valueTwo)
+{
+ pinI1 = 1;
+ pinI2 = 0;
+ pinI3 = 1;
+ pinI4 = 0;
+ ENA = valueOne;
+ ENB = valueTwo;
+}