Diff: main.cpp

Revision:

7:ebc76b0f21da

Parent:

5:40ac894e0fa7

Child:

8:c77ab7615b21

--- a/main.cpp	Sun Apr 05 01:50:25 2015 +0000
+++ b/main.cpp	Tue Apr 07 03:49:50 2015 +0000
@@ -1,7 +1,7 @@
 /**************************************************************************************************************************************************************
 //  Created by: Ryan Spillman
 //
-//  Last updated 4/4/2015
+//  Last updated 4/6/2015
 //
 //  This is the software for my teams autonomous boat that is our graduation/final project at Isothermal Community College 
 //
@@ -23,12 +23,7 @@
 #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 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
@@ -39,21 +34,20 @@
 
 PID headingPID(userKc, userTi, userTd, MAGN_PERIOD);    //Kc, Ti, Td, interval
 
-Timer t2;
+Timer GPSTimer;
 Timer headingTime;
 Timer acc;
 Timer magn;
 Timer inputTimer;
 
 //Prototype functions
-void updateMotorSpeed(char tempChar);
+void updateManualSpeed(char tempChar);
 void updateDir(char tempChar);
-void setGoalPos(float lat, float lon);
-void makeVector(float posZero, float posOne, float curPos[2]);
+void makeVector(double posZero, double posOne, double curPos[2]);
 //End of prototype functions
 
 //Enter new positions here
-float goalPos[10][2] = {    {35.478407, -81.981978},
+double goalPos[10][2] = {   {35.478407, -81.981978},
                             {35.478344, -81.981986},
                             {35.478407, -81.981978},
                             {35.478344, -81.981986},
@@ -63,11 +57,16 @@
                             {35.478407, -81.981978},
                             {35.478344, -81.981986},
                             {35.478407, -81.981978}
-                       };
+                        };
 
-float polarVector[2] =      {0,0};
+double polarVector[2] =     {0,0};
 float motorSpeed =          0.5f;
-float curPos[2] =           {0,0};
+float manualSpeed =         0.5f;
+double curPos[2] =          {0,0};
+
+/**************************************************************************************************************************************************************************************************
+//                                                  MAIN
+**************************************************************************************************************************************************************************************************/
 
 int main()
 {
@@ -84,33 +83,27 @@
     
     //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");
-    t2.start();
-    
-    //wait until we have a gps fix
-    while(gps.fixtype == 0)
+    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(t2.read() >= GPS_PERIOD)
+        if(GPSTimer.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();
+            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);
+    //gps.Sleep(1);
     
     //get IMU data and calculate the tilt compensated compass
     getAccel();
@@ -118,15 +111,15 @@
     updateAngles();
     
     //set current position
-    curPos[0] = gps.latitude;
-    curPos[1] = gps.longitude;
+    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.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+    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("starting main loop\n");
+    xBee.printf("\n\nstarting main loop\n");
         
-    //PID control of left and right motors based on input from gyro
+    //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);
@@ -141,25 +134,23 @@
     headingTime.start();
     acc.start();
     magn.start();
-    xBee.printf("Starting Motors!\n");
-    goForward(0.5f, 0.5f);
     int wakeUp = 1;
+    float leastHDOP = 100;
     while (1) 
     {
-        //manual mode
+        /**********************************************************************************************************************************************************************************************
+        //                                          manual mode
+        **********************************************************************************************************************************************************************************************/
+
         if(mode == 0)
         {
-            if(t2.read() >= GPS_PERIOD)
+            if(GPSTimer.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();
+                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();
             }
-            //checks to see if data is available on xBee
+            //check to see if data is available on xBee
             if(xBee.readable())
             {
                 char recChar = xBee.getc();
@@ -182,7 +173,7 @@
                         updateDir(recChar);
                     }
                     
-                    else updateMotorSpeed(recChar);
+                    else updateManualSpeed(recChar);
                     
                     //reset timer
                     inputTimer.reset();
@@ -202,21 +193,25 @@
                 //has the delay time elapsed since the first same char
                 else if(inputTimer.read_ms() >= DELAYTIME)
                 {
-                    updateMotorSpeed(recChar);
+                    updateManualSpeed(recChar);
                 }
             }
         }
-        //autonomous mode
+        
+        /**********************************************************************************************************************************************************************************************
+        //                                          autoznomous mode
+        **********************************************************************************************************************************************************************************************/
+
         if(mode == 1)
         {
             //only send wake up once
             if(wakeUp == 1)
             {
                 //wake up GPS
-                gps.Sleep(0);
+                //gps.Sleep(0);
                 wakeUp = 0;
             }
-            //Emergency stop
+            //check xBee
             if(xBee.readable())
             {
                 char recChar = xBee.getc();
@@ -224,41 +219,39 @@
                 {
                     xBee.printf("emergency stop\n");
                     goStop(1,1);
-                    while(1);
+                    mode = 0;
                 }
                 //change to manual mode
                 if(recChar == 'y')
                 {
                     mode = 0;
+                    wayPtNum = 0;
                 }
             }
             else
             {
-                if(t2.read() >= GPS_PERIOD)
+                if(GPSTimer.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();
                     
+                    //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;
                     
-                    //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
+                        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]);
-                    //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], yaw);
-                    t2.reset();
+                    GPSTimer.reset();
                 }
                 
                 if(acc.read() >= ACCEL_PERIOD)
@@ -271,17 +264,9 @@
                     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);
                     
@@ -300,24 +285,22 @@
                         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
+        
+        /**********************************************************************************************************************************************************************************************
+        //                                          record position mode
+        **********************************************************************************************************************************************************************************************/
+        
         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");
+            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')
             {
@@ -325,6 +308,25 @@
             }
             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];
@@ -375,120 +377,110 @@
                     goalPos[9][0] = curPos[0];
                     goalPos[9][1] = curPos[1];
                 }
-                xBee.printf("position %c updated\n", recChar);
+                xBee.printf("position %c updated\t", recChar);
             }
-            xBee.printf("exiting record mode\n");
+            xBee.printf("returning to manual mode\n\n");
             mode = 0;
         }
     }
 }
 
-void updateMotorSpeed(char tempChar)
+/**************************************************************************************************************************************************************************************************
+//                                                  Update PWM Value for Manual Mode
+**************************************************************************************************************************************************************************************************/
+
+void updateManualSpeed(char tempChar)
 {
     if(tempChar == '1')
         {
             goStop(1, 1);
-            xBee.printf("Stop\n%.2f\t", motorSpeed);
+            xBee.printf("Stop\n");
         }
         
     else if(tempChar == 'p')
     {
-        if(motorSpeed < 1)
+        if(manualSpeed < 1)
         {
-            motorSpeed = motorSpeed + 0.01f;
-            xBee.printf("Increasing MotorSpeed\n%.2f\t", motorSpeed);
+            manualSpeed = manualSpeed + 0.01f;
+            xBee.printf("Increasing manualSpeed\t%.2f\n", manualSpeed);
         }
-        else(xBee.printf("Maximum motorSpeed reached\n%.2f\t", motorSpeed));
+        else(xBee.printf("Maximum manualSpeed reached\t%.2f\n", manualSpeed));
         
-        //errors with floating point calculations are causing motorSpeed to exceed 1
+        //errors with floating point calculations are causing manualSpeed to exceed 1
         //this is a workaround
-        if(motorSpeed > 1)
+        if(manualSpeed > 1)
         {
-            motorSpeed = 1;
+            manualSpeed = 1;
         }
     }
     
     else if(tempChar == 'm')
     {
-        if(motorSpeed > 0.01f)
+        if(manualSpeed > 0.01f)
         {
-            motorSpeed = motorSpeed - 0.01f;
-            xBee.printf("MotorSpeed decreased\n%.2f\t", motorSpeed);
+            manualSpeed = manualSpeed - 0.01f;
+            xBee.printf("manualSpeed decreased\t%.2f\n", manualSpeed);
         }
         else
         {
-            //calculation errors may prevent motorSpeed from reaching actual zero
-            motorSpeed = 0;
-            xBee.printf("Minimum motorSpeed reached\n%.2f\t", motorSpeed);
+            //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(motorSpeed, motorSpeed);
-            //wait(time);
-            //goStop(1,1);
-            xBee.printf("Going forward\n%.2f\t", motorSpeed);
+            goForward(manualSpeed, manualSpeed);
+            xBee.printf("Going forward\t%.2f\n", manualSpeed);
         }
         else if(tempChar == 's')
         {
-            goBackward(motorSpeed, motorSpeed);
-            //wait(time);
-            //goStop(1,1);
-            xBee.printf("Going backward\n%.2f\t", motorSpeed);
+            goBackward(manualSpeed, manualSpeed);
+            xBee.printf("Going backward\t%.2f\n", manualSpeed);
         }
         else if(tempChar == 'd')
         {
-            goRight(motorSpeed, motorSpeed);
-            //wait(time);
-            //goStop(1,1);
-            xBee.printf("Going right\n%.2f\t", motorSpeed);
+            goRight(manualSpeed, manualSpeed);
+            xBee.printf("Going right\t%.2f\n", manualSpeed);
         }
         else if(tempChar == 'a')
         {
-            goLeft(motorSpeed, motorSpeed);
-            //wait(time);
-            //goStop(1,1);
-            xBee.printf("Going left\n%.2f\t", motorSpeed);
+            goLeft(manualSpeed, manualSpeed);
+            xBee.printf("Going left\t%.2f\n", manualSpeed);
         }
 }
 
+/**************************************************************************************************************************************************************************************************
+//                                                  create polar vector based on two sets of latitude and longitude
+**************************************************************************************************************************************************************************************************/
 
-//create polar vector based on two sets of latitude and longitude
-void makeVector(float posZero, float posOne, float curPos[2])
+void makeVector(double posZero, double posOne, double curPos[2])
 {
-    float arcLength[2];
-    float goalPos[2];
+    double arcLength[2];
+    double goalPos[2];
     goalPos[0] = posZero;
     goalPos[1] = posOne;
     
-    //arc length = radius * angle
+    /*Note: 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.)
+    //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]);
 }