Firmware and Simulator for controlling a vehicle with mecanum wheels