Altered GridMap to align with previous changes, altered Routing

General house keeping on Routing, changed GridMap to reflect change to
grid[][] declaration, added better demo for path finding in grid to
Working.ino