Init not working correctly, setMotorState not changing to correct state on the SM
Being able to initialize without using the setMotorState function