    int distance = getDistance(mCurrentWayPoint, flat, flon);
    
    //Check if distance is less then 2 meters
-   if(distance < MIN_DISTANCE){
+   if(distance <= MIN_DISTANCE){
      lcd2.setCursor(0,3);
      //Advance to next way point
      mCurrentWayPoint++;

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("**Finish Reached**"));
     } else {
       lcd2.print(F("**Waypoint Reached**"));
       delay(30000);
     }   
   } else {
     //Navigate to the way point
     //Adjust max amount to be off course
     int highTurnRange = 30;
     if(turning){
       highTurnRange = 10;
     } 
     //Base turning range off of distance
     int turnRange = constrain(map(distance, 0, 100, 5, 30), 5, highTurnRange);
     if(abs(compassHeading - heading) < turnRange){
       //Forward
       turning = false;
       lcd2.setCursor(0,3);
       lcd2.print(F("  Command: FORWARD"));
       forward(); 
     } else if (compassHeading > heading) {
       //Left
       turning = true;
       lcd2.setCursor(0,3);
       lcd2.print(F("  Command: LEFT")); 
       left();
     } else {
       //Right
       turning = true;
       lcd2.setCursor(0,3);
       lcd2.print(F("  Command: RIGHT"));
       right(); 
