Started with pathfinding and updated readme drastically. Added a test robot for testing pathfinding.