 const int REVSPEED        = 75;
 const int STOP_TIME       = 1500;//time to coast to stop
 const int DANGER_TIME     = STOP_TIME+800; //time to backup after last sighting
-const uint16_t warn[]     = {850, 1050, 2150, 1050, 850};
+const uint16_t warn[]     = {1000, 1600, 2400, 1600, 1000};
 const double POINT_RADIUS = .001; //in miles, margin for error in rover location
-const double PING_CALIB   = 1500.l;
+const double PING_CALIB   = 1800.l;
 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 double lineGravity  = .50; //line return factor
 
 HardwareSerial *CommSerial = &Serial;
 NMEA			nmea(Serial1);
 CommManager		manager(CommSerial);
 Point			location(0,0);
+Point			backWaypoint(0,0);
 HLA				lowFilter (600000, 0);//10 minutes
 HLA				highFilter(    10, 0);//10 milliseconds
 Servo			servo[3]; //drive, steer, backSteer

	if(manager.numWaypoints() <= 0) return;

	distance = calcDistance(manager.getTargetWaypoint(), location);
	pathHeading = calcHeading(location, manager.getTargetWaypoint());
	//if there is a waypoint path, find the line to drive from;
	//either going to waypoint != to the first
	//or looping is on
}

void readAccelerometer(){

	if(dT < 1000 && !nmea.getWarning()){ //ignore irrational values
		//3600000 = milliseconds per hour
		float dTraveled = nmea.getGroundSpeed()*dT/3600000.f;
		dTraveled *= (2.l/3.l);//purposly undershoot
		location = extrapPosition(location, trueHeading, dTraveled);
	}
	positionChanged();
