File content as of revision 7:ebc76b0f21da:

/**************************************************************************************************************************************************************
//  Created by: Ryan Spillman
//
//  Last updated 4/6/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 GPS_ALPHA       0.3f                            //GPS low pass alpha
#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 GPSTimer;
Timer headingTime;
Timer acc;
Timer magn;
Timer inputTimer;

//Prototype functions
void updateManualSpeed(char tempChar);
void updateDir(char tempChar);
void makeVector(double posZero, double posOne, double curPos[2]);
//End of prototype functions

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

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

/**************************************************************************************************************************************************************************************************
//                                                  MAIN
**************************************************************************************************************************************************************************************************/

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();
    wait(1);
    //Setup the GPS
    gps.Init();

    xBee.printf("gps initialized\n");
    
    xBee.printf("attempting to get a fix\n");
    GPSTimer.start();

    while(gps.fixtype != 2)                     //wait until we have a DGPS fix (more accurate)
    //while(gps.fixtype != 2)                   //wait for a GPS fix only (faster startup)
    {
        if(GPSTimer.read() >= GPS_PERIOD)
        {
            gps.parseData();
            xBee.printf("Waiting for DGPS fix\tcurrent fix = %d\n", gps.fixtype);
            GPSTimer.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.degLat;
    curPos[1] = gps.degLon;
    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.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
    xBee.printf("dist %f\theading %f\n", polarVector[0], polarVector[1]);
    xBee.printf("\n\nstarting main loop\n");
        
    //PID control of left and right motors based on input from tilt compensated compass
    //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();
    int wakeUp = 1;
    float leastHDOP = 100;
    while (1) 
    {
        /**********************************************************************************************************************************************************************************************
        //                                          manual mode
        **********************************************************************************************************************************************************************************************/

        if(mode == 0)
        {
            if(GPSTimer.read() >= GPS_PERIOD)
            {
                gps.parseData();
                xBee.printf("lat %f\tlon %f\tHDOP %f\tfix %d\tsat %d\tspd %f\n", gps.degLat, gps.degLon, gps.HDOP, gps.fixtype, gps.satellites, gps.speed);
                GPSTimer.reset();
            }
            //check 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 updateManualSpeed(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)
                {
                    updateManualSpeed(recChar);
                }
            }
        }
        
        /**********************************************************************************************************************************************************************************************
        //                                          autoznomous mode
        **********************************************************************************************************************************************************************************************/

        if(mode == 1)
        {
            //only send wake up once
            if(wakeUp == 1)
            {
                //wake up GPS
                //gps.Sleep(0);
                wakeUp = 0;
            }
            //check xBee
            if(xBee.readable())
            {
                char recChar = xBee.getc();
                if(recChar == 'n')
                {
                    xBee.printf("emergency stop\n");
                    goStop(1,1);
                    mode = 0;
                }
                //change to manual mode
                if(recChar == 'y')
                {
                    mode = 0;
                    wayPtNum = 0;
                }
            }
            else
            {
                if(GPSTimer.read() >= GPS_PERIOD)
                {
                    gps.parseData();
                    
                    //leastHDOP will reset every 5 readings
                    //keeps the most accurate reading
                    if(gps.HDOP < leastHDOP)
                    {
                        curPos[0] = gps.degLat;
                        curPos[1] = gps.degLon;
                    }
                    gpsCount = gpsCount + 1;
                    
                    if(gpsCount == 5)
                    {
                        makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
                        headingPID.reset();                                             //Reset PID data since we have a new heading
                        gpsCount = 0;
                        leastHDOP = 100;
                    }
                    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]);
                    GPSTimer.reset();
                }
                
                if(acc.read() >= ACCEL_PERIOD)
                {
                    getAccel();
                    acc.reset();
                }
                if(magn.read() >= MAGN_PERIOD)
                {
                    getMagn();
                    updateAngles();
                    magn.reset();
                    filtered = lowPass(yaw, filtered, 0);
                    magDiff = whichWay(filtered, polarVector[1]);
                    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);
                    }    
                }
            }
        }
        
        /**********************************************************************************************************************************************************************************************
        //                                          record position mode
        **********************************************************************************************************************************************************************************************/
        
        if(mode == 3)
        {
            xBee.printf("\nPlease enter position number (0-9), or press y to return to manual mode\n");
            
            while(!xBee.readable());
            
            char recChar = xBee.getc();
            recChar = xBee.getc();
            
            //return to manual mode
            if(recChar == 'y')
            {
                mode = 0;
            }
            else 
            {
                xBee.printf("\nFinding most accurate GPS position\nThis will take a few seconds\n\n");
            
                float lowestHDOP = 100;
                
                //take 25 GPS readings and keep the position with the lowest horizontal dilution of precision (HDOP)
                for(int i = 0; i< 50; i++)
                {
                    gps.parseData();
                    
                    if(gps.HDOP <= lowestHDOP)
                    {
                        lowestHDOP = gps.HDOP;
                        curPos[0] = gps.degLat;
                        curPos[1] = gps.degLon;
                    }
                    
                    while(GPSTimer.read() < GPS_PERIOD);
                    GPSTimer.reset();
                }
                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\t", recChar);
            }
            xBee.printf("returning to manual mode\n\n");
            mode = 0;
        }
    }
}

/**************************************************************************************************************************************************************************************************
//                                                  Update PWM Value for Manual Mode
**************************************************************************************************************************************************************************************************/

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

/**************************************************************************************************************************************************************************************************
//                                                  Update Direction for Manual Mode
**************************************************************************************************************************************************************************************************/

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

/**************************************************************************************************************************************************************************************************
//                                                  create polar vector based on two sets of latitude and longitude
**************************************************************************************************************************************************************************************************/

void makeVector(double posZero, double posOne, double curPos[2])
{
    double arcLength[2];
    double goalPos[2];
    goalPos[0] = posZero;
    goalPos[1] = posOne;
    
    /*Note: arc length = radius * angle*/
    //Y
    arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
    //X
    arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
    
    //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;
}