 void waypointUpdated(){
 	if(manager.numWaypoints() > 0 && !nmea.getWarning()){
 		stop = false;
-		distance = calcDistance(manager.getTargetWaypoint(), location);
-		if(distance > POINT_RADIUS) return;
+ 		distance = calcDistance(manager.getTargetWaypoint(), location);
+		if(distance > PointRadius)  return;
 
 		if(manager.getTargetIndex() < manager.numWaypoints()-1){
 			backWaypoint = manager.getTargetWaypoint();

	updateGPS();
	updateGyro();
	if(uTime <= millis()){
		uTime += SCHEDULE_DELAY;
		schedule[sIter]();
		sIter++;
		sIter = sIter%(sizeof(schedule)/sizeof(*schedule));

#include "HLAverage.h"
#include "wiring_private.h"

//Constants that should never change during driving and never/rarely tuned
const uint8_t VoltagePin  = 67;
const uint8_t LEDpin[]    = {25, 26, 27}; //blue, yellow, red
const uint8_t PingPin[]	  = {A4, A1, A0, A2, A3}; //left to right
const uint8_t ServoPin[]  = {12, 11,  8};//drive, steer, backS; APM 1,2,3 resp.
const uint8_t RadioPin[]  = {2, 3}; //drive, steer
const double  pAngle[5]   = { 79.27l, 36.83l, 0.l, -36.83l, -79.27l};
const double  dPlsb       = 4.f/float(0xffff); //dps Per leastSigBit for gyro
const int ScheduleDelay   = 22;
const uint16_t warn[]     = {1000, 1600, 2500, 1600, 1000};
const double PointRadius  = .001; //in miles, margin for error in rover location

//Global variables used throught the program
HardwareSerial *CommSerial = &Serial;
CommManager		manager(CommSerial);
NMEA			nmea(Serial1);
Point			location(0,0);
Point			backWaypoint(0,0);
HLA				lowFilter (600000, 0);//10 minutes
HLA				highFilter(    10, 0);//10 milliseconds
HLA 			pitch( 100, 0);
HLA 			roll ( 100, 0);
Servo			servo[3]; //drive, steer, backSteer
		//scheduler, navigation, obstacle, stop times
uint32_t uTime = 0, nTime = 0, oTime = 0, sTime = 0;
uint32_t gpsHalfTime = 0, gpsTime = 0;
 int32_t Ax,Ay,Az; //used in accelerometer calculations
uint16_t ping[5] = {20000,20000,20000,20000,20000};
uint8_t  sIter,pIter; //iterators for scheduler and ping
double   pathHeading; //All Headings are Clockwise+, -179 to 180, 0=north
double   trueHeading;
double   gyroHalf; //Store Gyro Heading halfway between gps points
double   distance;
boolean  stop = true;
boolean  backDir;
