private void updatePosition(Location location) { System.out.println("Updating position"); currentTime = System.currentTimeMillis() / 1000.0 - startTime + timeBuffer - offset; System.out.println("current time nearest" + currentTime); Waypoint desiredLocation = Waypoint.getNearestWaypoint(course, currentTime); System.out.println("nearest waypoint: " + desiredLocation.toString()); double lat = location.getLatitude(); double lon = location.getLongitude(); System.out.println("longitude: " + lon); double alt = location.getAltitude(); System.out.println("altitude: " + alt); Waypoint actual = new Waypoint(lat, lon, alt, currentTime, null, null); log.add(actual.toString()); // write log to file each time, or get rid of log entirely System.out.println("theta before" + desiredLocation.time); ErrorVector currentError = ErrorVector.errorVector(actual, desiredLocation); updateDials(actual, desiredLocation, currentError, location); }