Replaced waypoint with pathfinding