Add pos1 and pos2 commands
Add ClearCommand
Move GROUND_HEIGHT constant
Start adding RouteCommand
Add debug tools and work out some kinks
Adding WIP router
Start getting routing working
Add report