Setup first waypoint