        lcd2.print(F("**Finish Reached**"));
      } else {
        lcd2.print(F("**Waypoint Reached**"));
-       delay(2000);
+       delay(30000);
      }   
    } else {
      //Navigate to the way point
-     if(abs(compassHeading - heading) < 5){
+     //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(); 

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;

//Kalman filter for smoothing compass value
//Kalman compassFilter(0.125,32,1023,0); //suggested initial values for high noise filtering

/* true if in a turn */
boolean turning = false;
 
//Method definitions
void gpsdump(TinyGPS &gps);
bool feedgps();
