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

	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(){

	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
