update

components/eq_20_asb_motor/eq20_servomotor.cpp

@@ -9,7 +9,7 @@ using namespace iflytop;
 
 #define TAG "Eq20ServoMotor"
 
-void Eq20ServoMotor::init(ModbusBlockHost *modbusBlockHost, int moduleid, int motorid = 1) {
+void Eq20ServoMotor::init(ModbusBlockHost *modbusBlockHost, int moduleid, int motorid) {
   //  this->com = com;
   m_modbusBlockHost = modbusBlockHost;
   m_deviceId        = motorid;
@@ -230,18 +230,28 @@ int32_t Eq20ServoMotor::module_get_last_exec_status(int32_t *status) {
   *status = 0;
   return 0;
 }
-int32_t Eq20ServoMotor::module_get_status(int32_t *status) {}
+int32_t Eq20ServoMotor::module_get_status(int32_t *status) {
+  *status = 0;
+  servo_internal_status_t internal_state;
+  int32_t                 ecode = get_servo_internal_state(internal_state);
+  if (ecode != 0) {
+    return ecode;
+  }
+  if (!internal_state.data.sbit.is_reach_target) {
+    *status = 1;
+  }
+  return 0;
+}
 int32_t Eq20ServoMotor::module_get_error(int32_t *iserror) {
   *iserror = 0;
-
-  // get_servo_internal_state();
   servo_internal_status_t internal_state;
   int32_t                 ecode = get_servo_internal_state(internal_state);
   if (ecode != 0) {
     return ecode;
   }
-  *status = internal_state.data.sbit.servo_warning | internal_state.data.sbit.servo_warning;
-  return 0;
+  if (internal_state.data.sbit.servo_warning | internal_state.data.sbit.servo_warning) {
+    *iserror = err::kmodule_error;
+  }
   return 0;
 }
 int32_t Eq20ServoMotor::module_clear_error() { return err::koperation_not_support; }
@@ -254,7 +264,15 @@ 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_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return rotate(direction * motor_velocity, acc); }
+
+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) {}
-int32_t Eq20ServoMotor::motor_move_to_with_torque(int32_t pos, int32_t torque) {}
+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_move_to_with_torque(int32_t pos, 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); }
+
+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); }

components/eq_20_asb_motor/eq20_servomotor.hpp

@@ -120,6 +120,13 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor {
   virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override;
   virtual int32_t motor_move_to_with_torque(int32_t pos, int32_t torque) override;
 
+  virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override;
+  virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override;
+  virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override;
+
+  virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
+  virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
+
  public:
   int32_t write_reg(uint32_t regadd, int32_t value);
   int32_t read_reg(uint32_t regadd, int32_t &value);

components/hardware/uart/zuart_helper.cpp

@@ -3,8 +3,8 @@
 #include "sdk\components\zprotocols\errorcode\errorcode.hpp"
 
 using namespace iflytop;
-#define DEBUG 0
-#if DEBUG
+#define __DEBUG 0
+#if __DEBUG
 static void dumphex(char* tag, uint8_t* data, uint8_t len) {
   printf("%s:", tag);
   for (int i = 0; i < len; i++) {
@@ -30,7 +30,7 @@ void ZUARTHelper::cleanRxBuff() {
 }
 int32_t ZUARTHelper::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, uint16_t overtimems) {
   uint32_t enter_ticket = HAL_GetTick();
-#if DEBUG
+#if __DEBUG
   dumphex("tx:", tx, txdatalen);
 #endif
 
@@ -43,7 +43,7 @@ int32_t ZUARTHelper::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint
     osDelay(1);
     int rxsize = expectrxsize - __HAL_DMA_GET_COUNTER(m_hdma_rx);
     if (rxsize == expectrxsize) {
-#if DEBUG
+#if __DEBUG
       dumphex("rx:", rx, expectrxsize);
 #endif
       break;