David Spillman
Crude navigation
Dependencies: GPS L3GD20 LSM303DLHC mbed PID
Diff: main.cpp
- Revision:
- 2:503a5ac6c3b6
- Parent:
- 0:e79311aae7ed
- Child:
- 3:ffa0e1429a72
--- a/main.cpp Sat Feb 21 01:22:52 2015 +0000
+++ b/main.cpp Wed Mar 04 17:48:08 2015 +0000
@@ -1,41 +1,62 @@
#include "mbed.h"
#include "GPS.h"
#include "modSensData.h"
+#include "move.h"
+#include "PID.h"
//Radius of the earth in meters
#define EARTHRADIUS 6378100.0f
//Tolerance for heading actual and heading needed
-#define HEADDIFF 5.0f
+#define HEADDIFF 2.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 1
-
+#define PERIOD 0.5f
+//Period in seconds of PID loop
+#define RATE .001
+#define PIDCYCLES 100
//GPS
GPS gps(D9, D7);
//X-Bee
Serial xBee(PTC15, PTC14);
+//PID
+//Kc, Ti, Td, interval
+PID controller(.01, .01, 20, RATE);
-float startPos[2], curPos[2], goalPos[2] = {35.478593, -81.981878}, polarVector[2];
+//Enter new position here
+float goalPos[2] = {35.336020, -81.912420};
+
+float startPos[2], curPos[2], polarVector[2];
+
+int sCheck, pCheck = 1;
void setGoalPos(float lat, float lon);
void makeVector(void);
-char whichWay(float magHead, float calcHead);
+float whichWay(float magHead, float calcHead);
void testYaw(void);
int main()
{
- xBee.baud(9600);
+ 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);
+ controller.setMode(0);
+ //We want the difference to be zero
+ controller.setSetPoint(0);
- //uncomment to check math for calculating needed heading
- //testYaw();
+ xBee.baud(9600);
xBee.printf("\nI'm Alive...\n");
//Setup the GPS
gps.Init();
+ xBee.printf("gps initialized\n");
- gps.parseData();
+ xBee.printf("attempting to get a fix\n");
//wait until we have a gps fix
while(gps.fixtype == 0)
@@ -55,25 +76,59 @@
//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("starting main loop\n");
while (1)
{
- // Process / check GPS data
+ //Emergency stop
+ if(xBee.readable())
+ {
+ char tempChar = xBee.getc();
+ if(tempChar == 'n')
+ {
+ xBee.printf("emergency stop\n");
+ goStop(1,1);
+ while(1);
+ }
+ }
+ //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();
+ //makeVector();
+ //xBee.printf("Main 3\n");
+ //get data from IMU and do calculations to determine heading
+ //updateAngles();
+ //updateAngles();
- //get data from IMU and do calculations to determine heading
- updateAngles();
+ float latDif = sqrt((goalPos[0] - curPos[0]) * (goalPos[0] - curPos[0]));
+ float longDif = sqrt((goalPos[1] - curPos[1]) * (goalPos[1] - curPos[1]));
- //decide which way to go
- char direction = whichWay(yaw, polarVector[1]);
- 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]);
- wait(PERIOD);
+ //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())
+ {
+ 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]);
}
}
@@ -88,8 +143,6 @@
//X
arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
- //arcLength[0] = EARTHRADIUS * (curPos[1] - goalPos[1]);
- //arcLength[1] = EARTHRADIUS * (curPos[0] - goalPos[0]);
//calculate magnitude of vector
polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
@@ -100,32 +153,135 @@
if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
}
-//Decide which direction to turn based on current compass heading and heading required
-char whichWay(float magHead, float calcHead)
+//Decide which direction to turn based on current compass heading(magHead) and heading required(calcHead)
+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.0f - HEADDIFF)))
+ if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360 - HEADDIFF)))
{
- if(calcHead > magHead)
+ //quadrant I
+ if(calcHead < 90)
{
- if((calcHead - magHead) < 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
{
- return 'R';
+ 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;
+ }
}
- return 'L';
+ else
+ {
+ if(magHead < (180 + calcHead))
+ {
+ //turn left need negative
+ magDiff = calcHead - magHead;
+ }
+ else
+ {
+ //turn right need positive
+ magDiff = calcHead - magHead + 360;
+ }
+ }
}
- else if((magHead - calcHead) < 180)
+ //quadrant III
+ else if(calcHead < 270)
{
- return 'L';
+ 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;
+ }
+ }
}
- return 'R';
+ //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 'G';
+ return 0;
}
+
+
//Test for verifying heading
void testYaw(void)
{