move waypoint function into turtle, add axis priority