From 128f9465584404dd65e7dfe7c214e7fb06a7a459 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Sun, 14 Jul 2024 12:15:59 +0800
Subject: [PATCH] update

---
 a8000_protocol                                                   | 2 +-
 sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp | 6 ++++--
 sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp | 6 +-----
 3 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/a8000_protocol b/a8000_protocol
index 401f316..ec875a3 160000
--- a/a8000_protocol
+++ b/a8000_protocol
@@ -1 +1 @@
-Subproject commit 401f316f91ca1b0206f257a99fe0a727665c5220
+Subproject commit ec875a3f707e73bf00266b2349d55f98caf41263
diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
index 5e3aade..d1e50b5 100644
--- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
+++ b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
@@ -107,6 +107,7 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
     PROCESS_REG(kreg_step_motor_tzerowait, REG_GET(m_cfg.motor_tzerowait), REG_SET(m_cfg.motor_tzerowait));
     PROCESS_REG(kreg_step_motor_enc_resolution, REG_GET(m_cfg.motor_enc_resolution), REG_SET(m_cfg.motor_enc_resolution));
     PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc), REG_SET(m_cfg.motor_enable_enc));
+    PROCESS_REG(kreg_step_motor_dzero_pos, REG_GET(m_cfg.motor_dzero), REG_SET(m_cfg.motor_dzero));
 
     default:
       return err::kmodule_not_find_reg;
@@ -637,8 +638,9 @@ int32_t StepMotorCtrlModule::trysyncpos() {
   return 0;
 }
 int32_t StepMotorCtrlModule::setnowpos(int32_t x) {
-  m_stepM1->setXACTUAL(x);
-  m_stepM1->set_enc_val(x);
+  int32_t final_x = x+m_cfg.motor_dzero;
+  m_stepM1->setXACTUAL(final_x);
+  m_stepM1->set_enc_val(final_x);
   return 0;
 }
 int32_t StepMotorCtrlModule::moveTo(int32_t x, int32_t v) {
diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
index a56ccc5..525249e 100644
--- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
+++ b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
@@ -32,6 +32,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
     int32_t motor_tzerowait;
     int32_t motor_enc_resolution;
     int32_t motor_enable_enc;
+    int32_t motor_dzero;  // 零点偏移
   } config_t;
 
   typedef struct {
@@ -86,7 +87,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   virtual int32_t step_motor_easy_move_to_end_point() override;
   virtual int32_t step_motor_easy_reciprocating_motion(int32_t startpos, int32_t endpos, int32_t times) override;
 
-
   virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override;
   virtual int32_t step_motor_active_cfg() override;
   virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state) override;
@@ -99,8 +99,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
 
   virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) override;
 
-
-
  public:
   TMC51X0*  getMotor() { return m_stepM1; }
   config_t* getCfg() { return &m_cfg; }
@@ -116,7 +114,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   int32_t do_step_motor_easy_move_to_end_point();
 
  private:
-
   int inter_et_pos(int32_t& x);
 
   void befor_motor_move();
@@ -133,7 +130,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status);
   void setErrorFlag(int32_t ecode);
 
-
   int32_t getnowpos(int32_t& x);
   int32_t trysyncpos();
   int32_t setnowpos(int32_t x);