File content as of revision 0:e79311aae7ed:

#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);
    }
    */
}