David Spillman
Crude navigation
Dependencies: GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem
Fork of
GPSNavigation
by 
David Spillman
Diff: main.cpp
- Revision:
- 8:c77ab7615b21
- Parent:
- 7:ebc76b0f21da
- Child:
- 9:fb8e34e31dfb
--- a/main.cpp Tue Apr 07 03:49:50 2015 +0000
+++ b/main.cpp Mon Apr 27 16:49:48 2015 +0000
@@ -1,86 +1,157 @@
-/**************************************************************************************************************************************************************
+/************************************************************************************************************************************************************************************************/
// Created by: Ryan Spillman
//
// Last updated 4/6/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
+// GPS waypoints are stored on an external micro-SD card
+// The user can record new waypoints to the SD card by driving to a location and entering record mode
+// The user can also manually adjust the waypoints with a text editor
//
// 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
-**************************************************************************************************************************************************************/
+// MTK3339 GPS module,xBee Pro S1, three TE KRPA-11 relays (relay logic H-bridge for trolling motor), and a L298n (H-Bridge for linear actuator)
+//
+/***************************************************How To***************************************************************************************************************************************/
+//
+// 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
+// User can drive the vehicle in manual mode to any position
+// User can record current position to a selected goal position in record mode
+// In autonomous mode, the vehicle uses GPS data and compass data to navigate to each goal position
+//
+// Controls in manual mode:
+// directional:
+// w = forward
+// s = backward
+// a = left
+// d = right
+// mode:
+// r = change to record mode
+// z = change to autonomous mode
+//
+// Controls in autonomous mode:
+// mode:
+// y = change to manual mode
+// adjustments:
+// d = increase angle
+// a = decrease angle
+// r = enter new waypoint number
+// + = increase (depends on adjust mode)
+// - = decrease (depends on adjust mode)
+// p = change adjust mode
+//
+// Controls in record mode:
+// *follow serial prompts to record positions
+// mode:
+// y = change to manual mode
+//
+/*************************************************************************************************************************************************************************************************/
#include "mbed.h"
+#include "GPS.h"
#include "PID.h"
+#include "SDFileSystem.h"
#include "modSensData.h"
#include "navigation.h"
-#include "GPS.h"
+#include "Actuator.h"
+#include "TrollingMotor.h"
+#define VOLT_MULT (3.3f / (0.810f / (3.3f + 0.810f))) //voltage divider 3.3k and 810 (VREF = 3.3V) keeps 12V measurements below 3.3V
+#define RATIO_TOLERANCE 0.02f
+#define MIN_RATIO 0.04f //Actuator hits retract limit swithc at 2.2%
+#define MAX_RATIO 0.85f //Actuator hits extend limit switch at 87.6%
+#define CENTER_RATIO 0.29f //Ratio where prop is centered
+
+#define FIX 0 // 2 = DGPS (more accurate but slower to initialize) 1 = GPS only (less accurate but faster to initialize)
+#define ARRIVED 5.0f //Tolerance, in meters, for when goal location is reached
+#define GPS_ACCUR 3.0f //accuracy of GPS unit
+#define GPS_PERIOD 1.0f //GPS polling period (1 Hz)
+#define GPS_POLL 0.5f
#define GPS_ALPHA 0.3f //GPS low pass alpha
-#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
+
+#define RATE 0.3f
+#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 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
-PID headingPID(userKc, userTi, userTd, MAGN_PERIOD); //Kc, Ti, Td, interval
-
-Timer GPSTimer;
Timer headingTime;
Timer acc;
Timer magn;
Timer inputTimer;
+Timer loopTimer;
-//Prototype functions
-void updateManualSpeed(char tempChar);
-void updateDir(char tempChar);
-void makeVector(double posZero, double posOne, double curPos[2]);
-//End of prototype functions
+FILE *fr; //file pointer for SD card
+
+//On board microSD
+SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd", PTE6, SDFileSystem::SWITCH_POS_NO, 50000000);
+
+/***************************************************Prototype functions****************************************************************************************************************************/
+void getDist(double posZero, double posOne, double curPos[2]);
+void getAngle(double posZero, double posOne, double curPos[2], int flush);
+/***************************************************End of prototype functions*********************************************************************************************************************/
+
+/***************************************************Global Variables*******************************************************************************************************************************/
+double polarVector[2] = {0,0.0000001f};
+float manualSpeed = 0.5f;
+double curPos[2] = {0,0};
-//Enter new positions here
-double 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}
- };
+//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};
-float motorSpeed = 0.5f;
-float manualSpeed = 0.5f;
-double curPos[2] = {0,0};
-
-/**************************************************************************************************************************************************************************************************
+/*************************************************************************************************************************************************************************************************/
// MAIN
-**************************************************************************************************************************************************************************************************/
+/*************************************************************************************************************************************************************************************************/
int main()
{
- int wayPtNum = 0, mode = 0;
- char prevChar = 'z';
+ 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;
xBee.baud(115200);
xBee.printf("\nI'm Alive...\n");
xBee.printf("Press any key to begin\n");
- //wait for keypress
+ //wait for keypress to begin
while(!xBee.readable());
+ //start of SD card read
+ fr = fopen ("/sd/GPS_CORDS.txt", "rt");
+
+ 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]);
+ xBee.printf("waypoint %d = %f,%f\n", x, goalPos[x][0], goalPos[x][1]);
+ }
+ fclose(fr);
+ //end of SD card read
+
//initialize magnetometer, accelerometer
compass.init();
wait(1);
@@ -90,31 +161,22 @@
xBee.printf("gps initialized\n");
xBee.printf("attempting to get a fix\n");
- GPSTimer.start();
-
- while(gps.fixtype != 2) //wait until we have a DGPS fix (more accurate)
- //while(gps.fixtype != 2) //wait for a GPS fix only (faster startup)
+
+ while(gps.fixtype != FIX)
{
- if(GPSTimer.read() >= GPS_PERIOD)
+ while(!gps.getData());
+ if(gps.fixtype == 1)
{
- gps.parseData();
- xBee.printf("Waiting for DGPS fix\tcurrent fix = %d\n", gps.fixtype);
- GPSTimer.reset();
+ xBee.printf("Waiting for DGPS fix\tcurrent fix = GPS only\n");
+ 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);
}
+ else xBee.printf("Waiting for DGPS fix\tcurrent fix = no fix\n");
}
- //put GPS in sleep mode to prevent overflowing the buffer
- //gps.Sleep(1);
-
//get IMU data and calculate the tilt compensated compass
getAccel();
getMagn();
updateAngles();
- //set current position
- curPos[0] = gps.degLat;
- curPos[1] = gps.degLon;
- makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
-
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");
@@ -122,33 +184,62 @@
//PID control of left and right motors based on input from tilt compensated compass
//Goal is to have the vehicle go straight
headingPID.setInputLimits(-180, 180);
- headingPID.setOutputLimits(-0.5f, 0.5f);
+
+ //set proportional output limits based on physical limits of actuator and mounting error
+ float distFromCenter = calcEnds(CENTER_RATIO, MAX_RATIO, MIN_RATIO);
+
+ headingPID.setOutputLimits((CENTER_RATIO - distFromCenter), (CENTER_RATIO + distFromCenter));
//set mode to auto
headingPID.setMode(0);
//We want the difference to be zero
headingPID.setSetPoint(0);
- headingPID.setBias(0.1f);
- float filtered = 0.0000001f;
- int gpsCount = 0;
+ loopTimer.start();
headingTime.start();
acc.start();
magn.start();
- int wakeUp = 1;
- float leastHDOP = 100;
+
while (1)
{
- /**********************************************************************************************************************************************************************************************
+ /*********************************************************************************************************************************************************************************************/
// manual mode
- **********************************************************************************************************************************************************************************************/
+ /*********************************************************************************************************************************************************************************************/
if(mode == 0)
{
- if(GPSTimer.read() >= GPS_PERIOD)
+ 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
+ if(loopTimer.read() > RATE)
{
- 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();
+ potVoltage = pot.read();
+ batVoltage = battery.read();
+ //voltage = voltage * 3.3f;
+ voltRatio = potVoltage / batVoltage;
+
+ float absDiff = sqrt((prevSet - voltRatio) * (prevSet - voltRatio));
+ if(absDiff <= RATIO_TOLERANCE)
+ {
+ turnStop(1.0f, 1.0f);
+ //xBee.printf("done\n");
+ }
+ else if((prevSet - voltRatio) >= 0)
+ {
+ turnRight(1.0f, 1.0f);
+ //xBee.printf("turning right\n");
+ }
+ 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);
+ loopTimer.reset();
}
//check to see if data is available on xBee
if(xBee.readable())
@@ -158,119 +249,280 @@
//change to autonomous mode
if(recChar == 'z')
{
+ xBee.printf("Changing to autonomous mode\n");
+ goStop();
mode = 1;
}
//change to record mode
- if(recChar == 'r')
+ else if(recChar == 'r')
{
+ xBee.printf("Changing to record mode\n");
+ goStop();
mode = 3;
}
- //did we recieve a new char?
- else if(recChar != prevChar)
+ else if(recChar == '1')
+ {
+ xBee.printf("stop\n");
+ goStop();
+ }
+ else if(recChar == 'w')
{
- if(recChar != 'p' && recChar != 'm' && recChar != '1')
- {
- updateDir(recChar);
- }
-
- else updateManualSpeed(recChar);
-
- //reset timer
- inputTimer.reset();
- inputTimer.start();
+ goForward();
+ prevSet = CENTER_RATIO;
+ xBee.printf("Forward\n");
+ }
+ else if(recChar == 's')
+ {
+ goBackward();
+ prevSet = CENTER_RATIO;
+ xBee.printf("backward\n");
+ }
+ else if(recChar == 'd')
+ {
+ xBee.printf("large step right\n");
- //keep recieved char to compare to next recieved char
- prevChar = recChar;
-
+ //find the best step size since 0.1 step will go over the limit
+ if(prevSet + 0.1f > MAX_RATIO)
+ {
+ prevSet = prevSet + (MAX_RATIO - prevSet);
+ }
+ else
+ {
+ prevSet = prevSet + 0.1f;
+ }
+ xBee.printf("set = %f\n", prevSet);
}
-
- else if(recChar != 'p' && recChar != 'm' && recChar != '1')
+ //large step left
+ else if(recChar == 'a')
{
- updateDir(recChar);
+ xBee.printf("large step left\n");
+ //find the best step size since 0.1 step will go over the limit
+ if(prevSet - 0.1f < MIN_RATIO)
+ {
+ prevSet = prevSet - (prevSet - MIN_RATIO);
+ }
+ else
+ {
+ prevSet = prevSet - 0.1f;
+ }
+ xBee.printf("set = %f\n", prevSet);
}
-
- //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)
+ //small step right
+ else if(recChar == 'e')
{
- updateManualSpeed(recChar);
+ xBee.printf("small step right\n");
+ if(prevSet + 0.01f > MAX_RATIO)
+ {
+ prevSet = prevSet + (MAX_RATIO - prevSet);
+ }
+ else
+ {
+ prevSet = prevSet + 0.01f;
+ }
+ xBee.printf("set = %f\n", prevSet);
+ }
+ else if(recChar == 'q')
+ {
+ xBee.printf("Small step left\n");
+ if(prevSet - 0.01f < MIN_RATIO)
+ {
+ prevSet = prevSet - (prevSet - MIN_RATIO);
+ }
+ else
+ {
+ prevSet = prevSet - 0.01f;
+ }
+ xBee.printf("set = %f\n", prevSet);
}
}
}
- /**********************************************************************************************************************************************************************************************
- // autoznomous mode
- **********************************************************************************************************************************************************************************************/
+ /*********************************************************************************************************************************************************************************************/
+ // autonomous mode
+ /*********************************************************************************************************************************************************************************************/
if(mode == 1)
{
- //only send wake up once
- if(wakeUp == 1)
- {
- //wake up GPS
- //gps.Sleep(0);
- wakeUp = 0;
- }
//check xBee
if(xBee.readable())
{
char recChar = xBee.getc();
- if(recChar == 'n')
+ if(recChar == '1')
{
- xBee.printf("emergency stop\n");
- goStop(1,1);
- mode = 0;
+ xBee.printf("stop\n");
+ goStop();
+ }
+ if(recChar == 'w')
+ {
+ xBee.printf("forward\n");
+ goForward();
}
//change to manual mode
if(recChar == 'y')
{
+ xBee.printf("Changing to manual mode\n");
+ goStop();
mode = 0;
wayPtNum = 0;
}
+ //increase calculated heading (use this to tweak/cheat calculated heading)
+ else if(recChar == 'd')
+ {
+ polarVector[1] = polarVector[1] + 1;
+ xBee.printf("increased angle %f\n", polarVector[1]);
+ }
+ //reduce calculated heading (use this to tweak/cheat calculated heading)
+ else if(recChar == 'a')
+ {
+ polarVector[1] = polarVector[1] - 1;
+ xBee.printf("reduced angle %f\n", polarVector[1]);
+ }
+ else if(recChar == '+')
+ {
+ if(adjustMode == 0)
+ {
+ if(wayPtNum != 9)
+ {
+ wayPtNum ++;
+ xBee.printf("waypoint increased to %d\n", wayPtNum);
+ }
+ else
+ {
+ xBee.printf("maximum waypoint reached\n");
+ }
+ }
+ else if(adjustMode == 1)
+ {
+ float curKc = headingPID.getPParam();
+ float curTi = headingPID.getIParam();
+ float curTd = headingPID.getDParam();
+ curKc = curKc + 0.1f;
+ headingPID.setTunings(curKc, curTi, curTd);
+ xBee.printf("Kc set to %f\n", curKc);
+ }
+ else if(adjustMode == 2)
+ {
+ float curKc = headingPID.getPParam();
+ float curTi = headingPID.getIParam();
+ float curTd = headingPID.getDParam();
+ curTi = curTi + 0.1f;
+ headingPID.setTunings(curKc, curTi, curTd);
+ xBee.printf("Ti set to %f\n", curTi);
+ }
+ else if(adjustMode == 3)
+ {
+ float curKc = headingPID.getPParam();
+ float curTi = headingPID.getIParam();
+ float curTd = headingPID.getDParam();
+ curTd = curTd + 0.1f;
+ headingPID.setTunings(curKc, curTi, curTd);
+ xBee.printf("Td set to %f\n", curTd);
+ }
+ }
+ else if(recChar == '-')
+ {
+ if(adjustMode == 0)
+ {
+ if(wayPtNum != 0)
+ {
+ wayPtNum --;
+ xBee.printf("waypoint increased to %d\n", wayPtNum);
+ }
+ else
+ {
+ xBee.printf("minimum waypoint reached\n");
+ }
+ }
+ else if(adjustMode == 1)
+ {
+ float curKc = headingPID.getPParam();
+ float curTi = headingPID.getIParam();
+ float curTd = headingPID.getDParam();
+ curKc = curKc - 0.1f;
+ headingPID.setTunings(curKc, curTi, curTd);
+ xBee.printf("Kc set to %f\n", curKc);
+ }
+ else if(adjustMode == 2)
+ {
+ float curKc = headingPID.getPParam();
+ float curTi = headingPID.getIParam();
+ float curTd = headingPID.getDParam();
+ curTi = curTi - 0.1f;
+ headingPID.setTunings(curKc, curTi, curTd);
+ xBee.printf("Ti set to %f\n", curTi);
+ }
+ else if(adjustMode == 3)
+ {
+ float curKc = headingPID.getPParam();
+ float curTi = headingPID.getIParam();
+ float curTd = headingPID.getDParam();
+ curTd = curTd - 0.1f;
+ headingPID.setTunings(curKc, curTi, curTd);
+ xBee.printf("Td set to %f\n", curTd);
+ }
+ }
+ //change current waypoint number
+ else if(recChar == 'r')
+ {
+ goStop();
+ //wayPtNum = 0;
+ //xBee.printf("waypoint count reset\n");
+ xBee.printf("Please enter desired waypoint (0-9)\t or press r to reset to zero\n");
+ while(!xBee.readable());
+ char tempWS[2];
+ tempWS[0] = xBee.getc();
+ if(tempWS[0] == 'r')
+ {
+ wayPtNum = 0;
+ }
+ else
+ {
+ sscanf(tempWS, "%d", &wayPtNum);
+ xBee.printf("waypoint is now %d\n", wayPtNum);
+ }
+ }
+ 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();
+ if(recCharTemp == 'w')
+ {
+ adjustMode = 0;
+ }
+ else if(recCharTemp == 'c')
+ {
+ adjustMode = 1;
+ xBee.printf("Adjust mode set to Kc\tEnter + to increment and - to decrement Kc\n");
+ }
+ else if(recCharTemp == 'i')
+ {
+ adjustMode = 2;
+ xBee.printf("Adjust mode set to Ti\tEnter + to increment and - to decrement Ti\n");
+ }
+ else if(recCharTemp == 'd')
+ {
+ adjustMode = 3;
+ xBee.printf("Adjust mode set to Td\tEnter + to increment and - to decrement Td\n");
+ }
+ else
+ {
+ xBee.printf("No changes made\n");
+ }
+ }
}
else
{
- if(GPSTimer.read() >= GPS_PERIOD)
+ if(gps.getData())
{
- gps.parseData();
-
- //leastHDOP will reset every 5 readings
- //keeps the most accurate reading
- if(gps.HDOP < leastHDOP)
- {
- curPos[0] = gps.degLat;
- curPos[1] = gps.degLon;
- }
- gpsCount = gpsCount + 1;
+ double tempPos[2] = {0,0};
- if(gpsCount == 5)
- {
- makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
- headingPID.reset(); //Reset PID data since we have a new heading
- gpsCount = 0;
- leastHDOP = 100;
- }
- 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]);
- GPSTimer.reset();
- }
-
- if(acc.read() >= ACCEL_PERIOD)
- {
- getAccel();
- acc.reset();
- }
- if(magn.read() >= MAGN_PERIOD)
- {
- getMagn();
- updateAngles();
- magn.reset();
- filtered = lowPass(yaw, filtered, 0);
- magDiff = whichWay(filtered, polarVector[1]);
- headingPID.setProcessValue(magDiff);
- motorSpeed = headingPID.compute();
- goForward(0.5f - motorSpeed,0.5f + motorSpeed);
+ tempPos[0] = gps.degLat;
+ tempPos[1] = gps.degLon;
+ getDist(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos);
+ getAngle(goalPos[wayPtNum][0],goalPos[wayPtNum][1], tempPos, 0);
+ //xBee.printf("dist %f\tMagDiff %f\tHDOP = %f\n", polarVector[0], magDiff, gps.HDOP);
- //If it is less than tolerance for arriving, stop
if(polarVector[0] <= ARRIVED)
{
xBee.printf("Goal Position %d reached!\n", wayPtNum);
@@ -279,25 +531,95 @@
if(wayPtNum >= 6)
{
xBee.printf("Final Position Reached!\nShutting down\n");
- goStop(1,1);
- while(1);
+ goStop();
+ mode = 0;
+ //while(1);
+ }
+ else
+ {
+ //flush heading PID data since we have a new heading
+ headingPID.reset();
+ getAngle(goalPos[wayPtNum ][0],goalPos[wayPtNum][1], goalPos[wayPtNum - 1], 1);
+ xBee.printf("Moving to Goal Position %d\theading = %f\n", wayPtNum, polarVector[1]);
}
- xBee.printf("Moving to Goal Position %d\n", wayPtNum);
- }
+ }
+ }
+
+ if(acc.read() >= ACCEL_PERIOD)
+ {
+ getAccel();
+ acc.reset();
+ }
+ //Heading PID
+ if(magn.read() >= MAGN_PERIOD)
+ {
+ getMagn();
+ updateAngles();
+ filtered = lowPass(yaw, filtered, 0);
+ magDiff = whichWay(filtered, 0);
+ headingPID.setProcessValue(-magDiff);
+ curSet = headingPID.compute();
+ xBee.printf("ratio = %f\tcurset = %f\theading = %f\tdiff = %f\n", voltRatio, curSet, filtered, magDiff);
+ magn.reset();
+ }
+
+ //This moves actuator to the requested position
+ //This is proportional feedback only
+ if(loopTimer.read() > RATE)
+ {
+ potVoltage = pot.read();
+ batVoltage = battery.read();
+ //voltage = voltage * 3.3f;
+ voltRatio = potVoltage / batVoltage;
+
+ float absDiff = sqrt((curSet - voltRatio) * (curSet - voltRatio));
+ if(absDiff <= RATIO_TOLERANCE)
+ {
+ turnStop(1.0f, 1.0f);
+ xBee.printf("done\n");
+ }
+ else if((curSet - voltRatio) >= 0)
+ {
+ if(voltRatio > MAX_RATIO)
+ {
+ xBee.printf("Max Limit Reached\n");
+ turnStop(1.0f, 1.0f);
+ }
+ else
+ {
+ turnRight(1.0f, 1.0f);
+ xBee.printf("turning Right\n");
+ }
+ }
+ else
+ {
+ if(voltRatio < MIN_RATIO)
+ {
+ xBee.printf("Min Limit Reached\n");
+ turnStop(1.0f, 1.0f);
+ }
+ else
+ {
+ turnLeft(1.0f, 1.0f);
+ 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);
+ loopTimer.reset();
}
}
}
- /**********************************************************************************************************************************************************************************************
+ /*********************************************************************************************************************************************************************************************/
// record position mode
- **********************************************************************************************************************************************************************************************/
+ /*********************************************************************************************************************************************************************************************/
if(mode == 3)
{
+ goStop();
xBee.printf("\nPlease enter position number (0-9), or press y to return to manual mode\n");
while(!xBee.readable());
-
char recChar = xBee.getc();
recChar = xBee.getc();
@@ -312,9 +634,12 @@
float lowestHDOP = 100;
- //take 25 GPS readings and keep the position with the lowest horizontal dilution of precision (HDOP)
+ //take 50 GPS readings and keep the position with the lowest horizontal dilution of precision (HDOP)
+ //lower HDOP = less error
for(int i = 0; i< 50; i++)
{
+ //wait for data to be available
+ //while(!gps._UltimateGps.readable())
gps.parseData();
if(gps.HDOP <= lowestHDOP)
@@ -323,39 +648,94 @@
curPos[0] = gps.degLat;
curPos[1] = gps.degLon;
}
-
- while(GPSTimer.read() < GPS_PERIOD);
- GPSTimer.reset();
+ 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';
+ while(!gps.getData() && !(tempChar == '1'))
+ {
+ if(xBee.readable())
+ {
+ tempChar = xBee.getc();
+ i = 50;
+ }
+ }
}
if(recChar == '0')
{
goalPos[0][0] = curPos[0];
goalPos[0][1] = curPos[1];
+ //write new coords to SD card
+ fr = fopen("/sd/GPS_CORDS.txt", "w+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
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+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
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+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
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+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
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+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
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+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
else if(recChar == '6')
{
@@ -366,16 +746,40 @@
{
goalPos[7][0] = curPos[0];
goalPos[7][1] = curPos[1];
+ //write new coords to SD card
+ fr = fopen("/sd/GPS_CORDS.txt", "w+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
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+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
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+");
+
+ for(int x = 0; x<=9; x++)
+ {
+ fprintf(fr, "%f,%f\n", &goalPos[x][0], &goalPos[x][1]);
+ }
+ fclose(fr);
}
xBee.printf("position %c updated\t", recChar);
}
@@ -385,84 +789,14 @@
}
}
-/**************************************************************************************************************************************************************************************************
-// Update PWM Value for Manual Mode
-**************************************************************************************************************************************************************************************************/
+/*************************************************************************************************************************************************************************************************/
+// create polar vector based on two sets of latitude and longitude
+/*************************************************************************************************************************************************************************************************/
+//TODO:
+//getDist and getAngle need to be optimized
+//they were one function but had to be hacked apart
-void updateManualSpeed(char tempChar)
-{
- if(tempChar == '1')
- {
- goStop(1, 1);
- xBee.printf("Stop\n");
- }
-
- else if(tempChar == 'p')
- {
- if(manualSpeed < 1)
- {
- manualSpeed = manualSpeed + 0.01f;
- xBee.printf("Increasing manualSpeed\t%.2f\n", manualSpeed);
- }
- else(xBee.printf("Maximum manualSpeed reached\t%.2f\n", manualSpeed));
-
- //errors with floating point calculations are causing manualSpeed to exceed 1
- //this is a workaround
- if(manualSpeed > 1)
- {
- manualSpeed = 1;
- }
- }
-
- else if(tempChar == 'm')
- {
- if(manualSpeed > 0.01f)
- {
- manualSpeed = manualSpeed - 0.01f;
- xBee.printf("manualSpeed decreased\t%.2f\n", manualSpeed);
- }
- else
- {
- //calculation errors may prevent manualSpeed from reaching actual zero
- manualSpeed = 0;
- xBee.printf("Minimum manualSpeed reached\t%.2f\n", manualSpeed);
- }
- }
-}
-
-/**************************************************************************************************************************************************************************************************
-// Update Direction for Manual Mode
-**************************************************************************************************************************************************************************************************/
-
-void updateDir(char tempChar)
-{
- if(tempChar == 'w')
- {
- goForward(manualSpeed, manualSpeed);
- xBee.printf("Going forward\t%.2f\n", manualSpeed);
- }
- else if(tempChar == 's')
- {
- goBackward(manualSpeed, manualSpeed);
- xBee.printf("Going backward\t%.2f\n", manualSpeed);
- }
- else if(tempChar == 'd')
- {
- goRight(manualSpeed, manualSpeed);
- xBee.printf("Going right\t%.2f\n", manualSpeed);
- }
- else if(tempChar == 'a')
- {
- goLeft(manualSpeed, manualSpeed);
- xBee.printf("Going left\t%.2f\n", manualSpeed);
- }
-}
-
-/**************************************************************************************************************************************************************************************************
-// create polar vector based on two sets of latitude and longitude
-**************************************************************************************************************************************************************************************************/
-
-void makeVector(double posZero, double posOne, double curPos[2])
+void getDist(double posZero, double posOne, double curPos[2])
{
double arcLength[2];
double goalPos[2];
@@ -477,10 +811,35 @@
//calculate magnitude of vector
polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
+}
+
+void getAngle(double posZero, double posOne, double curPos[2], int flush)
+{
+ double tempAngle = 0;
+ double arcLength[2];
+ double goalPos[2];
+ goalPos[0] = posZero;
+ goalPos[1] = posOne;
- //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])));
+ /*Note: arc length = radius * angle*/
+ //Y
+ arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
+ //X
+ arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
- //make negative angles positive
- if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
-}
+ 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.)
+ polarVector[1] = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
+ //make negative angles positive
+ if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
+ }
+ else
+ {
+
+ tempAngle = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
+
+ if(tempAngle < 0) tempAngle = tempAngle + 360;
+ polarVector[1] = lowPass(tempAngle, polarVector[1], 3);
+ }
+}
\ No newline at end of file