eq20 add config

components/eq_20_asb_motor/eq20_servomotor.cpp

@@ -9,12 +9,13 @@ using namespace iflytop;
 
 #define TAG "Eq20ServoMotor"
 
-void Eq20ServoMotor::init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid) {
+void Eq20ServoMotor::init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid, servo_config_t servo_config) {
   //  this->com = com;
   m_modbusBlockHost = modbusBlockHost;
   m_deviceId        = motorid;
   m_moduleId        = moduleid;
   ZASSERT(m_modbusBlockHost != NULL);
+  m_servo_config = servo_config;
 }
 
 #define DO(exptr)                                  \
@@ -224,6 +225,8 @@ int32_t Eq20ServoMotor::getid(int32_t *id) {
   *id = m_moduleId;
   return 0;
 }
+int32_t Eq20ServoMotor::module_enable(int32_t enable) { return motor_enable(enable); }
+
 int32_t Eq20ServoMotor::module_stop() {
   m_final_action = kActionType_none;
   return stop();
@@ -324,3 +327,21 @@ int32_t Eq20ServoMotor::motor_move_to_zero_backward(int32_t findzerospeed, int32
 }
 
 int32_t Eq20ServoMotor::motor_read_pos(int32_t *pos) { return get_pos(*pos); }
+
+int32_t Eq20ServoMotor::motor_easy_rotate(int32_t direction) {  //
+  return motor_rotate_acctime(direction, m_servo_config.default_velocity, m_servo_config.default_acc_time_ms);
+};
+int32_t Eq20ServoMotor::motor_easy_move_by(int32_t distance) {  //
+  return motor_move_by_acctime(distance, m_servo_config.default_velocity, m_servo_config.default_acc_time_ms);
+};
+int32_t Eq20ServoMotor::motor_easy_move_to(int32_t position) {  //
+  return motor_move_to_acctime(position, m_servo_config.default_velocity, m_servo_config.default_acc_time_ms);
+};
+int32_t Eq20ServoMotor::motor_easy_move_to_zero(int32_t direction) {  //
+  if (direction > 0)
+    return move_to_zero_forward(m_servo_config.find_zero_velocity, m_servo_config.find_zero_velocity, m_servo_config.default_acc_time_ms);
+  else
+    return move_to_zero_backward(m_servo_config.find_zero_velocity, m_servo_config.find_zero_velocity, m_servo_config.default_acc_time_ms);
+};
+int32_t Eq20ServoMotor::motor_easy_set_current_pos(int32_t pos) { return err::koperation_not_support; };
+int32_t Eq20ServoMotor::motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return err::koperation_not_support; };

components/eq_20_asb_motor/eq20_servomotor.hpp

@@ -62,6 +62,12 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor {
     } data;
   } servo_internal_status_t;
 
+  typedef struct {
+    int32_t default_acc_time_ms = 1000;
+    int32_t default_velocity    = 800;
+    int32_t find_zero_velocity  = 100;
+  } servo_config_t;
+
   typedef enum {
     kActionType_none,
     kActionType_rotate,
@@ -77,11 +83,13 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor {
 
   final_action_t m_final_action = kActionType_none;
 
+  servo_config_t m_servo_config;
+
  public:
   Eq20ServoMotor(/* args */){};
   ~Eq20ServoMotor(){};
 
-  void init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid);
+  void init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid, servo_config_t servo_config);
 
   int32_t enable(int32_t enable);
 
@@ -105,6 +113,8 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor {
    *******************************************************************************/
 
   virtual int32_t getid(int32_t *id) override;
+  virtual int32_t module_enable(int32_t enable) override;
+
   virtual int32_t module_ping() { return 0; };
 
   virtual int32_t module_stop() override;
@@ -139,6 +149,13 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor {
 
   virtual int32_t motor_read_pos(int32_t *pos);
 
+  virtual int32_t motor_easy_rotate(int32_t direction);
+  virtual int32_t motor_easy_move_by(int32_t distance);
+  virtual int32_t motor_easy_move_to(int32_t position);
+  virtual int32_t motor_easy_move_to_zero(int32_t direction);
+  virtual int32_t motor_easy_set_current_pos(int32_t pos);
+  virtual int32_t motor_easy_move_to_io(int32_t ioindex, int32_t direction);
+
  public:
   int32_t write_reg(uint32_t regadd, int32_t value);
   int32_t read_reg(uint32_t regadd, int32_t &value);

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -74,7 +74,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
    *******************************************************************************/
   virtual int32_t module_ping() { return 0; };
 
-  virtual int32_t module_enable(int32_t enable) override { motor_enable(enable); };
+
 
   virtual int32_t motor_enable(int32_t enable) override;
   virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override;
@@ -105,4 +105,4 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
   int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val);
   int32_t do_action(int32_t actioncode);
 };
-}  // namespace iflytop
+}  // namespace iflytop