Added more pathfinding. Introduced Bug with capsules falling over