Diff: main.cpp

Revision:

0:e79311aae7ed

Child:

2:503a5ac6c3b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 21 01:14:46 2015 +0000
@@ -0,0 +1,226 @@
+#include "mbed.h"
+#include "GPS.h"
+#include "modSensData.h"
+
+//Radius of the earth in meters
+#define EARTHRADIUS 6378100.0f
+//Tolerance for heading actual and heading needed
+#define HEADDIFF 5.0f
+//Period in seconds of the the main loop
+#define PERIOD 1
+
+//GPS
+GPS gps(D9, D7);
+
+//X-Bee
+Serial xBee(PTC15, PTC14);
+
+
+float startPos[2], curPos[2], goalPos[2] = {35.478593, -81.981878}, polarVector[2];
+
+void setGoalPos(float lat, float lon);
+void makeVector(void);
+char whichWay(float magHead, float calcHead);
+void testYaw(void);
+
+int main()
+{
+    xBee.baud(9600);
+    
+    //uncomment to check math for calculating needed heading
+    //testYaw();
+    
+    xBee.printf("\nI'm Alive...\n");
+    
+    //Setup the GPS
+    gps.Init();
+    
+    gps.parseData();
+    
+    //wait until we have a gps fix
+    while(gps.fixtype == 0)
+    {
+        xBee.printf("fix %d\n", gps.fixtype);
+        gps.parseData();
+        wait(.2);    
+    }
+    
+    //set starting position
+    curPos[0] = gps.latitude;
+    curPos[1] = gps.longitude;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    //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]);
+    
+    while (1) 
+    {
+        // Process / 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);
+        
+        //update current position
+        curPos[0] = gps.latitude;
+        curPos[1] = gps.longitude;
+        
+        makeVector();
+        
+        //get data from IMU and do calculations to determine heading
+        updateAngles();
+        
+        //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);
+    }
+}
+
+//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]);
+    
+    //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]));
+    
+    //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;
+}
+
+//Decide which direction to turn based on current compass heading and heading required
+char whichWay(float magHead, float calcHead)
+{
+    
+    float absOfDiff = sqrt((calcHead - magHead) * (calcHead - magHead));
+    
+    //Is the heading off enough to care?
+    if((absOfDiff >= HEADDIFF) && (absOfDiff <= (360.0f - HEADDIFF)))
+    {
+        if(calcHead > magHead)
+        {
+            if((calcHead - magHead) < 180)
+            {
+                return 'R';
+            }
+            return 'L';
+        }
+        else if((magHead - calcHead) < 180)
+        {
+            return 'L';
+        }
+        return 'R';
+    }
+    return 'G';
+}
+
+//Test for verifying heading
+void testYaw(void)
+{
+    //set test position to indicate a necessary heading of:
+    curPos[0] = 34.833716f;
+    curPos[1] = -82.404188f;
+    
+    //due north
+    goalPos[0] = 35.833716f;
+    goalPos[1] = -82.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due north\n", polarVector[0], polarVector[1]);
+    
+    //due south
+    goalPos[0] = 33.833716f;
+    goalPos[1] = -82.404188f;
+    
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due south\n", polarVector[0], polarVector[1]);
+    
+    //due east
+    goalPos[0] = 34.833716f;
+    goalPos[1] = -81.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due east\n", polarVector[0], polarVector[1]);
+    
+    
+    //due west
+    goalPos[0] = 34.833716f;
+    goalPos[1] = -83.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees due west\n", polarVector[0], polarVector[1]);
+    
+    //north east
+    goalPos[0] = 35.833716f;
+    goalPos[1] = -81.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees north east\n", polarVector[0], polarVector[1]);
+    
+    
+    //north west
+    goalPos[0] = 35.833716f;
+    goalPos[1] = -83.404188f;
+    
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees north west\n", polarVector[0], polarVector[1]);
+    
+    //south east
+    goalPos[0] = 33.833716f;
+    goalPos[1] = -81.404188f;
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees south east\n", polarVector[0], polarVector[1]);
+    
+    
+    //south west
+    goalPos[0] = 33.833716f;
+    goalPos[1] = -83.404188f;
+    
+    
+    //Convert starting position and goal position to a vector
+    makeVector();
+    
+    xBee.printf("dist %fmeters\tangle %fdegrees south west\n", polarVector[0], polarVector[1]);
+    
+    while(1);
+    
+    /*
+    //below is useful when checking compass heading
+    while(1)
+    {
+        updateAngles();
+        char direction = whichWay(yaw, polarVector[1]);
+        //printf("accX %f\taccY %f\taccZ %f\tmagX %f\tmagY %f\tmagZ %f\troll %f\tpitch %f\tyaw %f\tturn %c\n", accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2], roll, pitch, yaw, direction);
+        wait(1);
+    }
+    */
+}
\ No newline at end of file