Implemented Pathfinding/collision avoidance