       lcd2.print(F("   --------------"));
   }
   void executeCommand(int cmd){
-    float heading;
-    int distance;
     switch(cmd){
        case CMD_SHOW_POSITION:
          //Display 1 current position menu

         delay(5000);
       break;
       case CMD_NAVIGATE:
         lcd.clear();
         lcd.setCursor(0,0);
         lcd.print(F("<   Navigating "));
         lcd.setCursor(0,1);
         lcd.print(F("<   Waypoints "));
         
         lcd2.clear();
         lcd2.setCursor(0,0);
         lcd2.print(F("--- Waypoint: "));
         lcd2.print(mCurrentWayPoint);
         lcd2.setCursor(16,0);
         lcd2.print(F(" ---"));
         lcd2.setCursor(0,1);
         lcd2.print(F("MAG:"));
         lcd2.print(compassHeading);
         
         heading = getHeading(mCurrentWayPoint, flat, flon);  
         distance = getDistance(mCurrentWayPoint, flat, flon);
         
         //Check if distance is less then 2 meters
         if(distance < 2){
           //Advance to next way point
           mCurrentWayPoint++;
           lcd2.setCursor(0,3);
           lcd2.print(F("**Waypoint Reached**"));
         } else {
           //Navigate to the way point
           if(abs(compassHeading - heading) < 5){
             lcd2.setCursor(0,3);
             lcd2.print(F("  Command: FORWARD"));
             forward(); 
           } else if (compassHeading > heading) {
             lcd2.setCursor(0,3);
             lcd2.print(F("  Command: LEFT")); 
             left();
           } else {
             lcd2.setCursor(0,3);
             lcd2.print(F("  Command: RIGHT"));
             right(); 
           }
         }
         lcd2.setCursor(11,1);
         lcd2.print(F("G:"));
         lcd2.print(heading);
         lcd2.setCursor(0,2);
         lcd2.print(F("Distance: "));
         lcd2.print(formatDistance(distance));
       break;
       case CMD_ADD_WAY_POINT:
         lcd.clear();


TinyGPS gps;
SoftwareSerial mySerial(2, 255);    //used for gps rx and tx pins in use

//Minimum distance in meters to get to the current waypoint before heading to the next
#define MIN_DISTANCE 1

//Compass Stuff
LSM303 compass;
