added waypointDelta function to autonomous

also added distance check when calculating distance and rotation
distance.

changed most floats to double.