Merge pull request #2 from RoboGroup3/master

merge to haso
now navigate to closes unexplored space
Merge pull request #13 from hasofanten/master

Now possible to navigate to closest unexplored space