File content as of revision 4:a397b44a0fe8:

#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 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);

//X-Bee
Serial xBee(PTD3, PTD2);       //boat
//Serial xBee(PTC15, PTC14);   //car

//PID
//Kc, Ti, Td, interval
PID controller(.1, .005, .01, RATE);

Timer t2;

//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);
float whichWay(float magHead, float calcHead);
void testYaw(void);

int main()
{
    xBee.baud(9600);
    
    xBee.printf("\nI'm Alive...\n");
    
    //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");
    
    //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]);
    
    
    xBee.printf("starting main loop\n");
    while (1) 
    {
        //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();
        //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())
        {
            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)
            {
                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;
                }
            }
        }
        //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;
}