File content as of revision 5:40ac894e0fa7:

/**************************************************************************************************************************************************************
//  Created by: Ryan Spillman
//
//  Last updated 4/4/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
//
//  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
**************************************************************************************************************************************************************/

#include "mbed.h"
#include "PID.h"
#include "modSensData.h"
#include "navigation.h"
#include "GPS.h"

#define EARTHRADIUS 6378100.000000000000000000f         //Radius of the earth in meters
#define ARRIVED         2.0f                            //Tolerance for whether or not vehicle has arrived at goal position
#define MAGN_PERIOD     (1 / 15.0f)                     //magnetometer polling period (15 Hz) must manually update hardware ODR registers if chaging this value
#define ACCEL_PERIOD    (1 / 50.0f)                     //accelerometer polling period (50 Hz) must manually update hardware ODR registers if chaging this value
#define GPS_PERIOD       (1 / 5.0f)                     //GPS polling period (5 Hz) must manually update hardware ODR registers if chaging this value
#define GPS_ALPHA       0.3f
#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

Serial xBee(PTB11, PTB10);                              //UART 3
GPS gps(PTC15, PTC14);                                  //UART 4

PID headingPID(userKc, userTi, userTd, MAGN_PERIOD);    //Kc, Ti, Td, interval

Timer t2;
Timer headingTime;
Timer acc;
Timer magn;
Timer inputTimer;

//Prototype functions
void updateMotorSpeed(char tempChar);
void updateDir(char tempChar);
void setGoalPos(float lat, float lon);
void makeVector(float posZero, float posOne, float curPos[2]);
//End of prototype functions

//Enter new positions here
float 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}
                       };

float polarVector[2] =      {0,0};
float motorSpeed =          0.5f;
float curPos[2] =           {0,0};

int main()
{
    int wayPtNum = 0, mode = 0;
    char prevChar = 'z';
    float magDiff = 0;
    
    xBee.baud(115200);
    xBee.printf("\nI'm Alive...\n");
    xBee.printf("Press any key to begin\n");
    
    //wait for keypress
    while(!xBee.readable());
    
    //initialize magnetometer, accelerometer
    compass.init();
    
    //Setup the GPS
    gps.Init();

    xBee.printf("gps initialized\n");
    
    xBee.printf("attempting to get a fix\n");
    t2.start();
    
    //wait until we have a gps fix
    while(gps.fixtype == 0)
    {
        if(t2.read() >= GPS_PERIOD)
        {
            //xBee.printf("fix %d\n", gps.fixtype);
            gps.parseData();
            xBee.printf("fix %d\n", gps.fixtype);
            getAccel();
            
            //xBee.printf("fix %d\n", gps.fixtype);
            xBee.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);
            //wait(GPSPERIOD);
            t2.reset();
        }
    }
    //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.latitude;
    curPos[1] = gps.longitude;
    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.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
    xBee.printf("dist %f\theading %f\n", polarVector[0], polarVector[1]);
    xBee.printf("starting main loop\n");
        
    //PID control of left and right motors based on input from gyro
    //Goal is to have the vehicle go straight
    headingPID.setInputLimits(-180, 180);
    headingPID.setOutputLimits(-0.5f, 0.5f);
    //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;
    headingTime.start();
    acc.start();
    magn.start();
    xBee.printf("Starting Motors!\n");
    goForward(0.5f, 0.5f);
    int wakeUp = 1;
    while (1) 
    {
        //manual mode
        if(mode == 0)
        {
            if(t2.read() >= GPS_PERIOD)
            {
                //check GPS data
                gps.parseData();
                curPos[0] = curPos[0] + GPS_ALPHA * (gps.latitude - curPos[0]);
                curPos[1] = curPos[1] + GPS_ALPHA * (gps.longitude - curPos[1]);
                xBee.printf("Current Postion:\t%f\t%f\n", curPos[0], curPos[1]);
                
                t2.reset();
            }
            //checks to see if data is available on xBee
            if(xBee.readable())
            {
                char recChar = xBee.getc();
                
                //change to autonomous mode
                if(recChar == 'z')
                {
                    mode = 1;
                }
                //change to record mode
                if(recChar == 'r')
                {
                    mode = 3;
                }
                //did we recieve a new char?
                else if(recChar != prevChar)
                {
                    if(recChar != 'p' && recChar != 'm' && recChar != '1')
                    {
                        updateDir(recChar);
                    }
                    
                    else updateMotorSpeed(recChar);
                    
                    //reset timer
                    inputTimer.reset();
                    inputTimer.start();
                    
                    //keep recieved char to compare to next recieved char
                    prevChar = recChar;
                    
                }
                
                else if(recChar != 'p' && recChar != 'm' && recChar != '1')
                {
                    updateDir(recChar);
                }
                
                //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)
                {
                    updateMotorSpeed(recChar);
                }
            }
        }
        //autonomous mode
        if(mode == 1)
        {
            //only send wake up once
            if(wakeUp == 1)
            {
                //wake up GPS
                gps.Sleep(0);
                wakeUp = 0;
            }
            //Emergency stop
            if(xBee.readable())
            {
                char recChar = xBee.getc();
                if(recChar == 'n')
                {
                    xBee.printf("emergency stop\n");
                    goStop(1,1);
                    while(1);
                }
                //change to manual mode
                if(recChar == 'y')
                {
                    mode = 0;
                }
            }
            else
            {
                if(t2.read() >= GPS_PERIOD)
                {
                    //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);
                    curPos[0] = curPos[0] + GPS_ALPHA * (gps.latitude - curPos[0]);
                    curPos[1] = curPos[1] + GPS_ALPHA * (gps.longitude - curPos[1]);
                    t2.reset();
                    
                    gpsCount = gpsCount + 1;
                    
                    //low pass filter 5 gps data sets before recalculating heading and reseting PID data (should make for less oscillation)
                    if(gpsCount == 5)
                    {
                        makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
                        //latDif = sqrt((goalPos[wayPtNum][0] - curPos[0]) * (goalPos[wayPtNum][0] - curPos[0]));
                        //longDif = sqrt((goalPos[wayPtNum][1] - curPos[1]) * (goalPos[wayPtNum][1] - curPos[1]));
                        //xBee.printf("\n\nupdated vector\n\n");
                        
                        headingPID.reset(); //Reset PID data since we have a new heading
                        gpsCount = 0;
                    }
                    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]);
                    //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], yaw);
                    t2.reset();
                }
                
                if(acc.read() >= ACCEL_PERIOD)
                {
                    getAccel();
                    acc.reset();
                }
                if(magn.read() >= MAGN_PERIOD)
                {
                    getMagn();
                    updateAngles();
                    magn.reset();
                    //compassAvg();
                
                    filtered = lowPass(yaw, filtered, 0);
                    //xBee.printf("yaw = %f\tfiltered = %f\n", yaw, filtered);
                    //set heading to a fixed number for testing
                    magDiff = whichWay(filtered, polarVector[1]);
                    //magDiff = whichWay(yaw, polarVector[1]);
                    //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], yaw);
                    //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], filtered);
                    headingPID.setProcessValue(magDiff);
                    
                    motorSpeed = headingPID.compute();
                    goForward(0.5f - motorSpeed,0.5f + motorSpeed);
                    
                    //If it is less than tolerance for arriving, stop 
                    if(polarVector[0] <= ARRIVED)
                    {
                        xBee.printf("Goal Position %d reached!\n", wayPtNum);
                        wait(1);
                        wayPtNum ++;
                        if(wayPtNum >= 6)
                        {
                            xBee.printf("Final Position Reached!\nShutting down\n");
                            goStop(1,1);
                            while(1);
                        }
                        xBee.printf("Moving to Goal Position %d\n", wayPtNum);
                    }    
                }
                
                //xBee.printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n\n", polarVector[0], magDiff, gps.latitude, gps.longitude, goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
                //printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n\n", polarVector[0], magDiff, gps.latitude, gps.longitude, goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
                
                //wait for elapsed time to be greater than the period of the heading loop
                //while(headingTime.read() < RATE);
                //headingTime.reset();
            }
        }
        //record position
        if(mode == 3)
        {
            xBee.printf("Please enter position number (0-9), or press y to return to manual mode\nenter selection again if confirmation does not appear\n");
            
            while(!xBee.readable());
            
            char recChar = xBee.getc();
            recChar = xBee.getc();
            //return to manual mode
            if(recChar == 'y')
            {
                mode = 0;
            }
            else 
            {
                if(recChar == '0')
                {
                    goalPos[0][0] = curPos[0];
                    goalPos[0][1] = curPos[1];
                }
                else if(recChar == '1')
                {
                    goalPos[1][0] = curPos[0];
                    goalPos[1][1] = curPos[1];
                }
                else if(recChar == '2')
                {
                    goalPos[2][0] = curPos[0];
                    goalPos[2][1] = curPos[1];
                }
                else if(recChar == '3')
                {
                    goalPos[3][0] = curPos[0];
                    goalPos[3][1] = curPos[1];
                }
                else if(recChar == '4')
                {
                    goalPos[4][0] = curPos[0];
                    goalPos[4][1] = curPos[1];
                }
                else if(recChar == '5')
                {
                    goalPos[5][0] = curPos[0];
                    goalPos[5][1] = curPos[1];
                }
                else if(recChar == '6')
                {
                    goalPos[6][0] = curPos[0];
                    goalPos[6][1] = curPos[1];
                }
                else if(recChar == '7')
                {
                    goalPos[7][0] = curPos[0];
                    goalPos[7][1] = curPos[1];
                }
                else if(recChar == '8')
                {
                    goalPos[8][0] = curPos[0];
                    goalPos[8][1] = curPos[1];
                }
                else if(recChar == '9')
                {
                    goalPos[9][0] = curPos[0];
                    goalPos[9][1] = curPos[1];
                }
                xBee.printf("position %c updated\n", recChar);
            }
            xBee.printf("exiting record mode\n");
            mode = 0;
        }
    }
}

void updateMotorSpeed(char tempChar)
{
    if(tempChar == '1')
        {
            goStop(1, 1);
            xBee.printf("Stop\n%.2f\t", motorSpeed);
        }
        
    else if(tempChar == 'p')
    {
        if(motorSpeed < 1)
        {
            motorSpeed = motorSpeed + 0.01f;
            xBee.printf("Increasing MotorSpeed\n%.2f\t", motorSpeed);
        }
        else(xBee.printf("Maximum motorSpeed reached\n%.2f\t", motorSpeed));
        
        //errors with floating point calculations are causing motorSpeed to exceed 1
        //this is a workaround
        if(motorSpeed > 1)
        {
            motorSpeed = 1;
        }
    }
    
    else if(tempChar == 'm')
    {
        if(motorSpeed > 0.01f)
        {
            motorSpeed = motorSpeed - 0.01f;
            xBee.printf("MotorSpeed decreased\n%.2f\t", motorSpeed);
        }
        else
        {
            //calculation errors may prevent motorSpeed from reaching actual zero
            motorSpeed = 0;
            xBee.printf("Minimum motorSpeed reached\n%.2f\t", motorSpeed);
        }
    }
}

void updateDir(char tempChar)
{
        if(tempChar == 'w')
        {
            goForward(motorSpeed, motorSpeed);
            //wait(time);
            //goStop(1,1);
            xBee.printf("Going forward\n%.2f\t", motorSpeed);
        }
        else if(tempChar == 's')
        {
            goBackward(motorSpeed, motorSpeed);
            //wait(time);
            //goStop(1,1);
            xBee.printf("Going backward\n%.2f\t", motorSpeed);
        }
        else if(tempChar == 'd')
        {
            goRight(motorSpeed, motorSpeed);
            //wait(time);
            //goStop(1,1);
            xBee.printf("Going right\n%.2f\t", motorSpeed);
        }
        else if(tempChar == 'a')
        {
            goLeft(motorSpeed, motorSpeed);
            //wait(time);
            //goStop(1,1);
            xBee.printf("Going left\n%.2f\t", motorSpeed);
        }
}


//create polar vector based on two sets of latitude and longitude
void makeVector(float posZero, float posOne, float curPos[2])
{
    float arcLength[2];
    float goalPos[2];
    goalPos[0] = posZero;
    goalPos[1] = posOne;
    
    //arc length = radius * angle
    //Y
    //arcLength[1] = EARTHRADIUS * (sqrt((goalPos[0] * goalPos[0]))  - sqrt((curPos[0] * curPos[0])));
    //arcLength[1] = EARTHRADIUS * (sqrt((goalPos[0] - curPos[0]) * (goalPos[0] - curPos[0])));
    arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
    //xBee.printf("%f\n", arcLength[1]);
    //X
    //arcLength[0] = EARTHRADIUS * (sqrt((goalPos[1] - curPos[1]) * (goalPos[1] - curPos[1])));
    arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
    //printf("G0 = %f\t G1 = %f\tC0 = %f\tC1 = %f\tA0 = %f\tA1 = %f\n", goalPos[0], goalPos[1], curPos[0], curPos[1], arcLength[0], arcLength[1]);
    //xBee.printf("%f\n", goalPos[1]);
    //xBee.printf("%f\n", curPos[1]);
    
    //xBee.printf("%f\n", arcLength[0]);
    //calculate magnitude of vector
    polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
    //xBee.printf("%f\n", polarVector[0]);
    
    //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])));

    //printf("0 = %f\t1 = %f\theading = %f\n", arcLength[0], arcLength[1], polarVector[1]);

    //make negative angles positive
    if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
    //xBee.printf("%f\n", polarVector[1]);
}