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

const uint8_t ServoPin[]  = {12, 11,  8};//drive, steer, backS; APM 1,2,3 resp.
const uint8_t RadioPin[]  = {2, 3}; //drive, steer
const int SCHEDULE_DELAY  = 25;
const int STEERTHROW	  = 30; //degrees from far turn to center
const int REVTHROW		  = 20;
const int MIN_FWD		  = 105;
const int MAX_FWD		  = 115;
const int REVSPEED        = 75;
const int STOP_TIME       = 1500;//time to coast to stop

		outputAngle = toDeg(atan2(y,x))+90;
		bound(double(90-STEERTHROW), outputAngle, double(90+STEERTHROW));

		/*int */speed = (distance*5280.l);
		speed += 90;
		bound(MIN_FWD, speed, MAX_FWD);

