V511

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;
-}
+}
+
+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;
+}

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);
 

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