 
 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;

      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:
         navigate();
       break;
       case CMD_ADD_WAY_POINT:
         lcd.clear();
