fix some bug

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); }

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(){};