Added code to track to all the waypoints, luffing when done