Restructured navbot algorithm; waypoint grid is now floodfilled out from start position