Init not working correctly, setMotorState not changing to correct state on the SM