Diff: main.cpp

Revision:

5:40ac894e0fa7

Parent:

4:a397b44a0fe8

Child:

7:ebc76b0f21da

--- a/main.cpp	Tue Mar 17 01:13:17 2015 +0000
+++ b/main.cpp	Sun Apr 05 01:45:29 2015 +0000
@@ -1,292 +1,494 @@
-#include "mbed.h"
-#include "GPS.h"
-#include "modSensData.h"
-#include "move.h"
-#include "PID.h"
+/**************************************************************************************************************************************************************
+//  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
+**************************************************************************************************************************************************************/
 
-//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);
+#include "mbed.h"
+#include "PID.h"
+#include "modSensData.h"
+#include "navigation.h"
+#include "GPS.h"
 
-//X-Bee
-Serial xBee(PTD3, PTD2);       //boat
-//Serial xBee(PTC15, PTC14);   //car
+#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
 
-//PID
-//Kc, Ti, Td, interval
-PID controller(.1, .005, .01, RATE);
+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;
 
-//Enter new position here
-float goalPos[2] = {35.336020, -81.912420};
-
-float startPos[2], curPos[2], polarVector[2];
+//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
 
-int sCheck, pCheck = 1;
+//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}
+                       };
 
-void setGoalPos(float lat, float lon);
-void makeVector(void);
-float whichWay(float magHead, float calcHead);
-void testYaw(void);
+float polarVector[2] =      {0,0};
+float motorSpeed =          0.5f;
+float curPos[2] =           {0,0};
 
 int main()
 {
-    xBee.baud(9600);
+    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");
     
-    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");
+    t2.start();
     
     //wait until we have a gps fix
     while(gps.fixtype == 0)
     {
-        xBee.printf("fix %d\n", gps.fixtype);
-        gps.parseData();
-        wait(.2);    
+        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);
     
-    //set starting position
+    //get IMU data and calculate the tilt compensated compass
+    getAccel();
+    getMagn();
+    updateAngles();
+    
+    //set current position
     curPos[0] = gps.latitude;
     curPos[1] = gps.longitude;
-    
-    //Convert starting position and goal position to a vector
-    makeVector();
+    makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
     
-    //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("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);
     
-    
-    xBee.printf("starting main loop\n");
+    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) 
     {
-        //Emergency stop
-        if(xBee.readable())
+        //manual mode
+        if(mode == 0)
         {
-            char tempChar = xBee.getc();
-            if(tempChar == 'n')
+            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())
             {
-                xBee.printf("emergency stop\n");
-                goStop(1,1);
-                while(1);
+                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);
+                }
             }
         }
-        //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())
+        //autonomous mode
+        if(mode == 1)
         {
-            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)
+            //only send wake up once
+            if(wakeUp == 1)
+            {
+                //wake up GPS
+                gps.Sleep(0);
+                wakeUp = 0;
+            }
+            //Emergency stop
+            if(xBee.readable())
             {
-                if(magHead < (180 + calcHead))
+                char recChar = xBee.getc();
+                if(recChar == 'n')
                 {
-                    //turn left need negative
-                    magDiff = calcHead - magHead;
+                    xBee.printf("emergency stop\n");
+                    goStop(1,1);
+                    while(1);
                 }
-                else
+                //change to manual mode
+                if(recChar == 'y')
                 {
-                    //turn right need positive
-                    magDiff = calcHead - magHead + 360;
+                    mode = 0;
                 }
             }
             else
             {
-                if(magHead < (180 + calcHead))
-                {
-                    //turn left need negative
-                    magDiff = calcHead - magHead;
-                }
-                else
+                if(t2.read() >= GPS_PERIOD)
                 {
-                    //turn right need positive
-                    magDiff = calcHead - magHead + 360;
+                    //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();
                 }
-            }
-        }
-        //quadrant II
-        else if(calcHead < 180)
-        {
-            if(calcHead < magHead)
-            {
-                if(magHead < (180 + calcHead))
+                
+                if(acc.read() >= ACCEL_PERIOD)
+                {
+                    getAccel();
+                    acc.reset();
+                }
+                if(magn.read() >= MAGN_PERIOD)
                 {
-                    //turn left need negative
-                    magDiff = calcHead - magHead;
-                }
-                else
-                {
-                    //turn right need positive
-                    magDiff = calcHead - magHead + 360;
+                    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);
+                    }    
                 }
-            }
-            else
-            {
-                if(magHead < (180 + calcHead))
-                {
-                    //turn left need negative
-                    magDiff = calcHead - magHead;
-                }
-                else
-                {
-                    //turn right need positive
-                    magDiff = calcHead - magHead + 360;
-                }
+                
+                //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();
             }
         }
-        //quadrant III
-        else if(calcHead < 270)
+        //record position
+        if(mode == 3)
         {
-            if(calcHead < magHead)
+            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')
             {
-                if(magHead < (180 + calcHead))
-                {
-                    //turn left need negative
-                    magDiff = calcHead - magHead;
-                }
-                else
-                {
-                    //turn right need positive
-                    magDiff = calcHead - magHead + 360;
-                }
+                mode = 0;
             }
-            else
+            else 
             {
-                if(magHead < (180 + calcHead))
+                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')
                 {
-                    //turn left need negative
-                    magDiff = calcHead - magHead;
+                    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
+                else if(recChar == '8')
+                {
+                    goalPos[8][0] = curPos[0];
+                    goalPos[8][1] = curPos[1];
+                }
+                else if(recChar == '9')
                 {
-                    //turn right need positive
-                    magDiff = calcHead - magHead + 360;
+                    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);
         }
-        //quadrant IV
+        
+    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
         {
-            
-            if(calcHead < magHead)
-            {
-                magDiff = calcHead - magHead;
-            }
-            else
-            {
-                if(magHead < (calcHead - 180))
-                {
-                    
-                    magDiff = calcHead - 360 - magHead;
-                }
-                else
-                {
-                    //turn right need positive
-                    magDiff = calcHead - magHead;
-                }
-            }
+            //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);
         }
-        return magDiff;
-    }
-    return 0;
-}
\ No newline at end of file
+}
+
+
+//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]);
+}