 		double AB    = calcHeading(backWaypoint, manager.getTargetWaypoint());
 		double AL    = calcHeading(backWaypoint, location);
 		double d     = cos(toRad(AL-AB))*calcDistance(backWaypoint, location);
-		double D     = d + (full-d)*(1.l-lineGravity);
+		double D     = d + (full-d)*(1.l-LINEGRAVITY);
 		Point target = extrapPosition(backWaypoint, AB, D);
 		pathHeading  = calcHeading(location, target);
 	}

				outputAngle = atan( angularError*PI/180.l )*(2*STEERTHROW/PI);
				break;
			case 1:
				outputAngle = ((angularError/180.l)*(angularError/180.l))*(2.l*STEERTHROW);
				if(angularError < 0) outputAngle *= -1;
				break;
			default:

							reportLocation,
							};

//These parameters are synced with the dashboard in real time
//The defaults are set during setup and sent to the dash when a connection is
//made; after that they can be changed in the dash and changes will
//be sent to the robot.
#define LINEGRAVITY  manager.getFloat(10)
#define STEERTHROW   manager.getInt  (11)
#define STEER_STYLE  manager.getInt  (12)
#define STEER_FACTOR manager.getFloat(13)
#define MIN_FWD      manager.getInt  (14)
#define MAX_FWD      manager.getInt  (15)
#define REVTHROW     manager.getInt  (16)
#define REVSPEED     manager.getInt  (17)
#define PING_WEIGHT  manager.getFloat(18)
#define COAST_TIME   manager.getInt  (19)
#define DANGER_TIME  manager.getInt  (20)+manager.getInt(19)
void setTuningDefaults(){
	manager.setData(10, .50);  //LINEGRAVITY
	manager.setData(11, 45);   //STEERTHROW
	manager.setData(12, 1);    //STEER_STYLE
	manager.setData(13, 1.0);  //STEER_FACTOR
	manager.setData(14, 107);  //MIN_FWD
	manager.setData(15, 115);  //MAX_FWD
	manager.setData(16, 20);   //REVERSE_STEER_THROW
	manager.setData(17, 75);   //REVERSE_SPEED
	manager.setData(18, 1400.);//PING_WEIGHT
	manager.setData(19, 1500); //COAST_TIME
	manager.setData(20, 800);  //Minumum Backup Time
}

void setup() {
	Serial1.begin(38400);
	CommSerial->begin(Protocol::BAUD_RATE);
	InitMPU();
	pinMode(40, OUTPUT); digitalWrite(40, HIGH); //SPI select pin
	for(int i=0; i<3; i++) pinMode(LEDpin[i], OUTPUT);
	for(int i=0; i<3; i++) servo[i].attach(ServoPin[i]);
	output(90,90);
