enterModeandWait
add waitMotorReady