 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(distance > POINT_RADIUS) return;

		if(manager.getTargetIndex() < manager.numWaypoints()-1){
			//backWaypoint = manager.getTargetWaypoint();
			manager.advanceTargetIndex();
		}
		else if (manager.loopWaypoints()){
			//backWaypoint = manager.getTargetWaypoint();
			manager.setTargetIndex(0);
		}
		else{

	float voltage = float(analogRead(67)/1024.l*5.l*10.1l);
	manager.sendDataMessage(Protocol::DATA_LATITUDE, location.degLatitude());
	manager.sendDataMessage(Protocol::DATA_LONGITUDE, location.degLongitude());
	manager.sendDataMessage(Protocol::DATA_ROLL, roll*180/PI);
	manager.sendDataMessage(Protocol::DATA_PITCH, pitch*180/PI);
	manager.sendDataMessage(Protocol::DATA_HEADING, trueHeading);
	manager.sendDataMessage(Protocol::DATA_SPEED, nmea.getGroundSpeed());
	manager.sendDataMessage(Protocol::DATA_DISTANCE, voltage);
}

void calibrateGyro(){ //takes one second
