From 2db89032fa2947c7f5e32406051acd107b15bf11 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Sat, 20 Jul 2024 09:33:33 +0800
Subject: [PATCH] V511

---
 .../step_motor_ctrl_module.cpp                     | 26 +++++++++++++++++-----
 .../step_motor_ctrl_module.hpp                     |  2 ++
 usrc/project_configs.h                             |  2 +-
 3 files changed, 24 insertions(+), 6 deletions(-)

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 091bc83..d750d3d 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
@@ -46,7 +46,8 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) {
  *                                                         EXT                                                         *
  ***********************************************************************************************************************/
 int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) {
-  int32_t suc = pri_module_xxx_reg(param_id, read, val);
+  int32_t oldval = val;
+  int32_t suc    = pri_module_xxx_reg(param_id, read, val);
   if (suc != 0) return suc;
   if (!read) {
     switch (param_id) {
@@ -71,6 +72,12 @@ int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
       case kreg_step_motor_enc_resolution:
         step_motor_active_cfg();
         break;
+
+      case kreg_step_motor_dzero_pos:
+        //
+
+
+        break;
       default:
         break;
     }
@@ -107,7 +114,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));
+    PROCESS_REG(kreg_step_motor_dzero_pos, REG_GET(m_cfg.motor_dzero), update_dzero(m_cfg.motor_dzero));
 
     default:
       return err::kmodule_not_find_reg;
@@ -591,7 +598,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_zero() {
         befor_motor_move();
         exec_move_to_io_task(0, -1);
         after_motor_move();
-        setnowpos(0);
+        setnowpos(0 + m_cfg.motor_dzero);
       },
       [this]() { m_stepM1->stop(); });
   return 0;
@@ -638,7 +645,7 @@ int32_t StepMotorCtrlModule::trysyncpos() {
   return 0;
 }
 int32_t StepMotorCtrlModule::setnowpos(int32_t x) {
-  int32_t final_x = x + m_cfg.motor_dzero;
+  int32_t final_x = x;
   m_stepM1->setXACTUAL(final_x);
   m_stepM1->set_enc_val(final_x);
   return 0;
@@ -651,4 +658,13 @@ int32_t StepMotorCtrlModule::moveTo(int32_t x, int32_t v) {
 int32_t StepMotorCtrlModule::rotate(int32_t direction, int32_t v) {
   m_stepM1->moveToEnd(direction, v);
   return 0;
-}
\ No newline at end of file
+}
+
+int32_t StepMotorCtrlModule::update_dzero(int32_t dzero) {
+  int32_t now;
+  getnowpos(now);
+  int32_t nowabs    = now - m_cfg.motor_dzero;
+  m_cfg.motor_dzero = dzero;
+  setnowpos(nowabs + m_cfg.motor_dzero);
+  return 0;
+}
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 525249e..0b55a5f 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
@@ -137,6 +137,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   int32_t moveBy(int32_t dx, int32_t v);
   int32_t rotate(int32_t direction, int32_t v);
 
+  int32_t update_dzero(int32_t dzero);
+
   // int32_t read_enc_val(int32_t& enc_val);
   // int32_t write_enc_val(int32_t enc_val);
 
diff --git a/usrc/project_configs.h b/usrc/project_configs.h
index 3803b3f..2bef6bd 100644
--- a/usrc/project_configs.h
+++ b/usrc/project_configs.h
@@ -1,5 +1,5 @@
 #pragma once
-#define PC_VERSION           510
+#define PC_VERSION           511
 #define PC_MANUFACTURER      "http://www.iflytop.com/"
 #define PC_PROJECT_NAME      "a8000_subboard"
 #define PC_IFLYTOP_ENABLE_OS 1