Changed data types to float, added debug messages.
Reset now resets the simulation (including robot initial position.