From 0d5a3a53c8d3b3d22f5c3963dc94abadab11da7e Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Wed, 3 Jan 2024 10:54:05 +0800
Subject: [PATCH] =?UTF-8?q?eq20=20=E6=B7=BB=E5=8A=A0=E5=AF=84=E5=AD=98?=
 =?UTF-8?q?=E5=99=A8=E9=85=8D=E7=BD=AE=E9=80=9F=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 components/eq_20_asb_motor/eq20_servomotor.cpp | 16 ++++++++++++++--
 components/eq_20_asb_motor/eq20_servomotor.hpp |  2 ++
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp
index df95dde..d6f7281 100644
--- a/components/eq_20_asb_motor/eq20_servomotor.cpp
+++ b/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); }
diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp
index c3a833e..3ca5d2f 100644
--- a/components/eq_20_asb_motor/eq20_servomotor.hpp
+++ b/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;