Mas velocidad y mas correcion en la detection de cuando se llega a un
waypoint