From e30c3da5f2ea4b38e3f95580a1c12867521f2272 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 25 Oct 2023 15:47:43 +0800 Subject: [PATCH] fix some bug --- components/eq_20_asb_motor/eq20_servomotor.cpp | 76 ++++++++++++++++++++------ components/eq_20_asb_motor/eq20_servomotor.hpp | 9 +++ 2 files changed, 69 insertions(+), 16 deletions(-) diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index e8bd42e..9b681f2 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -224,8 +224,14 @@ int32_t Eq20ServoMotor::getid(int32_t *id) { *id = m_moduleId; return 0; } -int32_t Eq20ServoMotor::module_stop() { return stop(); } -int32_t Eq20ServoMotor::module_break() { return stop(); } +int32_t Eq20ServoMotor::module_stop() { + m_final_action = kActionType_none; + return stop(); +} +int32_t Eq20ServoMotor::module_break() { + m_final_action = kActionType_none; + return stop(); +} int32_t Eq20ServoMotor::module_get_last_exec_status(int32_t *status) { *status = 0; return 0; @@ -237,11 +243,19 @@ int32_t Eq20ServoMotor::module_get_status(int32_t *status) { if (ecode != 0) { return ecode; } - if (!internal_state.data.sbit.is_reach_target) { - *status = 1; - } - if (!internal_state.data.sbit.is_zero_complete) { - *status = 1; + + if (m_final_action == kActionType_rotate) { + if (internal_state.data.sbit.motor_is_rotating) { + *status = 1; + } + } else if (m_final_action == kActionType_moveTo) { + if (!internal_state.data.sbit.is_reach_target) { + *status = 1; + } + } else if (m_final_action == kActionType_moveToZero) { + if (!internal_state.data.sbit.is_zero_complete) { + *status = 1; + } } return 0; } @@ -260,24 +274,54 @@ int32_t Eq20ServoMotor::module_get_error(int32_t *iserror) { int32_t Eq20ServoMotor::module_clear_error() { return err::koperation_not_support; } int32_t Eq20ServoMotor::module_set_param(int32_t param_id, int32_t param_value) { return err::koperation_not_support; } int32_t Eq20ServoMotor::module_get_param(int32_t param_id, int32_t *param_value) { return err::koperation_not_support; } - int32_t Eq20ServoMotor::module_readio(int32_t *io) { return get_io_state(*io); } int32_t Eq20ServoMotor::module_writeio(int32_t io) { return err::koperation_not_support; } - 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_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; } int32_t Eq20ServoMotor::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } int32_t Eq20ServoMotor::motor_rotate_with_torque(int32_t direction, int32_t torque) { return err::koperation_not_support; } -int32_t Eq20ServoMotor::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return rotate(direction * motor_velocity, acctime); }; -int32_t Eq20ServoMotor::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return move_by(distance, motor_velocity, acctime); }; -int32_t Eq20ServoMotor::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return move_to(position, motor_velocity, acctime); } +#define AUTO_RSENT(exptr) \ + int32_t ecode = 0; \ + ecode = exptr; \ + for (size_t i = 0; i < 3; i++) { \ + if (ecode == 0) { \ + break; \ + } \ + ecode = exptr; \ + } +int32_t Eq20ServoMotor::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { + module_stop(); + AUTO_RSENT(rotate(direction * motor_velocity, acctime)); + if (ecode == 0) m_final_action = kActionType_rotate; + return ecode; +}; +int32_t Eq20ServoMotor::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { + module_stop(); + AUTO_RSENT(move_by(distance, motor_velocity, acctime)); + if (ecode == 0) m_final_action = kActionType_moveTo; + return ecode; +}; +int32_t Eq20ServoMotor::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { + module_stop(); + AUTO_RSENT(move_to(position, motor_velocity, acctime)); + if (ecode == 0) m_final_action = kActionType_moveTo; + return ecode; +} -int32_t Eq20ServoMotor::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to_zero_forward(findzerospeed, findzeroedge_speed, 30); } -int32_t Eq20ServoMotor::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to_zero_backward(findzerospeed, findzeroedge_speed, 30); } +int32_t Eq20ServoMotor::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { + module_stop(); + AUTO_RSENT(move_to_zero_forward(findzerospeed, findzeroedge_speed, 30)); + if (ecode == 0) m_final_action = kActionType_moveToZero; + return ecode; +} +int32_t Eq20ServoMotor::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { + module_stop(); + AUTO_RSENT(move_to_zero_backward(findzerospeed, findzeroedge_speed, 30)); + if (ecode == 0) m_final_action = kActionType_moveToZero; + return ecode; +} int32_t Eq20ServoMotor::motor_read_pos(int32_t *pos) { return get_pos(*pos); } diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index d74a34a..3ce662a 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -62,12 +62,21 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { } data; } servo_internal_status_t; + typedef enum { + kActionType_none, + kActionType_rotate, + kActionType_moveToZero, + kActionType_moveTo, + } final_action_t; + private: ModbusBlockHost *m_modbusBlockHost; int32_t m_deviceId = 0; int32_t m_moduleId = 0; int32_t m_auto_resendtimes = 3; + final_action_t m_final_action = kActionType_none; + public: Eq20ServoMotor(/* args */){}; ~Eq20ServoMotor(){};