Pathfinding + following now works