Updated pathfinding

No more stucking in the middle