Wrote pathfinding method for robots.

Included additional tests