Fixed map algorithm & preliminary tests of mapping with robot motion

Holy crap this is awesome