Mapping: Started implementing robot movement