Added motorstate, minor fixes with tests