Added IDLE case into comanMotionControl while changing into POSITION_CONTROL_MODE