Waypoints in place. Still Need Rotation