simpleMovement: Fixed various errors.
simpleMovement now works

Did various bugfixes in the simpleMovement and arduino code.