RedRob-R1A07, Fixed bug in state machine for motor control