Pathfinding working!