eq20 添加寄存器配置速度

components/eq_20_asb_motor/eq20_servomotor.cpp

@@ -275,8 +275,20 @@ int32_t Eq20ServoMotor::module_get_error(int32_t *iserror) {
   return 0;
 }
 int32_t Eq20ServoMotor::module_clear_error() { return err::koperation_not_support; }
-int32_t Eq20ServoMotor::module_set_reg(int32_t param_id, int32_t param_value) { return err::koperation_not_support; }
-int32_t Eq20ServoMotor::module_get_reg(int32_t param_id, int32_t *param_value) { return err::koperation_not_support; }
+int32_t Eq20ServoMotor::module_set_reg(int32_t param_id, int32_t param_value) { return module_xxx_reg(param_id, false, param_value); }
+int32_t Eq20ServoMotor::module_get_reg(int32_t param_id, int32_t *param_value) { return module_xxx_reg(param_id, true, *param_value); }
+int32_t Eq20ServoMotor::module_xxx_reg(int32_t param_id, bool read, int32_t &val) {
+  switch (param_id) {
+    MODULE_COMMON_PROCESS_REG_CB();
+    PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_servo_config.default_velocity), REG_SET(m_servo_config.default_velocity));
+    PROCESS_REG(kreg_module_input_state, module_readio(&val), ACTION_NONE);
+    PROCESS_REG(kreg_robot_pos, motor_read_pos(&val), ACTION_NONE);
+    default:
+      return err::kmodule_not_find_config_index;
+      break;
+  }
+  return 0;
+}
 int32_t Eq20ServoMotor::module_readio(int32_t *io) { return get_io_state(*io); }
 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); }

components/eq_20_asb_motor/eq20_servomotor.hpp

@@ -10,6 +10,7 @@ namespace iflytop {
 using namespace std;
 
 class Eq20ServoMotor : public ZIModule, public ZIMotor {
+  ENABLE_MODULE(Eq20ServoMotor, kmotor_module, 0x0001);
  public:
   typedef struct {
     union {
@@ -126,6 +127,7 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor {
 
   virtual int32_t module_set_reg(int32_t param_id, int32_t param_value) override;
   virtual int32_t module_get_reg(int32_t param_id, int32_t *param_value) override;
+  virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val) override;
 
   virtual int32_t module_readio(int32_t *io) override;