diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index b7db3fb..e8bd42e 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -9,7 +9,7 @@ using namespace iflytop; #define TAG "Eq20ServoMotor" -void Eq20ServoMotor::init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid ) { +void Eq20ServoMotor::init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid) { // this->com = com; m_modbusBlockHost = modbusBlockHost; m_deviceId = motorid; @@ -154,14 +154,14 @@ int32_t Eq20ServoMotor::get_servo_internal_state(servo_internal_status_t &state) return 0; } -#define AUTO_RESEND(exptr) \ - int32_t ret = 0; \ +#define AUTO_RESEND(exptr) \ + int32_t ret = 0; \ for (int i = 0; i < m_auto_resendtimes; i++) { \ - ret = exptr; \ - if (ret == 0) { \ - return 0; \ - } \ - } \ + ret = exptr; \ + if (ret == 0) { \ + return 0; \ + } \ + } \ return ret; int32_t Eq20ServoMotor::write_reg(uint32_t regadd, int32_t value) { // @@ -240,6 +240,9 @@ int32_t Eq20ServoMotor::module_get_status(int32_t *status) { if (!internal_state.data.sbit.is_reach_target) { *status = 1; } + if (!internal_state.data.sbit.is_zero_complete) { + *status = 1; + } return 0; } int32_t Eq20ServoMotor::module_get_error(int32_t *iserror) { @@ -263,8 +266,7 @@ int32_t Eq20ServoMotor::module_writeio(int32_t io) { return err::koperation_not_ int32_t Eq20ServoMotor::module_read_adc(int32_t adcindex, int32_t *adc) { return err::koperation_not_support; } -int32_t Eq20ServoMotor::motor_enable(int32_t varenable) { - return enable(varenable); } +int32_t Eq20ServoMotor::motor_enable(int32_t varenable) { return enable(varenable); } int32_t Eq20ServoMotor::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } int32_t Eq20ServoMotor::motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; }