From 964d71283c9d80e642c332491235d3f4e243f86e Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Wed, 26 Jun 2024 09:57:48 +0800
Subject: [PATCH] update

---
 components/step_motor_ctrl_module/step_motor_ctrl_module.cpp | 9 +++++++++
 components/step_motor_ctrl_module/step_motor_ctrl_module.hpp | 1 +
 components/zcancmder/zcan_protocol_parser.cpp                | 7 +++++++
 components/zcancmder/zcan_protocol_parser.hpp                | 1 +
 4 files changed, 18 insertions(+)

diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
index bb2ce1f..1597382 100644
--- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
+++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
@@ -531,6 +531,15 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_end_point() {
   }
   return do_step_motor_easy_move_to_end_point();
 }
+int32_t StepMotorCtrlModule::step_motor_read_pos_and_enc_pos(int32_t* pos, int32_t* encpos) {
+  *pos = m_stepM1->getXACTUAL();
+  if (m_cfg.motor_enable_enc_resolution != 0) {
+    *encpos = m_stepM1->read_enc_val();
+  } else {
+    *encpos = *pos;
+  }
+  return 0;
+}
 
 int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
   m_thread.stop();
diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
index db90d8f..96814ff 100644
--- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
+++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
@@ -87,6 +87,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   virtual int32_t step_motor_easy_move_to_zero() override;
   virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) override;
   virtual int32_t step_motor_easy_move_to_end_point() override;
+  virtual int32_t step_motor_read_pos_and_enc_pos(int32_t* pos, int32_t* encpos) override;
 
   virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override;
   virtual int32_t step_motor_active_cfg() override;
diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp
index a116f97..7a90d2a 100644
--- a/components/zcancmder/zcan_protocol_parser.cpp
+++ b/components/zcancmder/zcan_protocol_parser.cpp
@@ -41,6 +41,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
   REGFN(step_motor_read_io_index_in_stm32);
   REGFN(step_motor_set_subdevice_reg);
   REGFN(step_motor_get_subdevice_reg);
+  REGFN(step_motor_read_pos_and_enc_pos);
 
   REGFN(mini_servo_enable);
   REGFN(mini_servo_read_pos);
@@ -334,6 +335,12 @@ int32_t ZCanProtocolParser::step_motor_get_subdevice_reg(cmdcontxt_t* cxt) {
   cxt->acklen  = 4;
   return module->step_motor_get_subdevice_reg(cxt->params[0], ack);
 }
+int32_t ZCanProtocolParser::step_motor_read_pos_and_enc_pos(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(0);
+  int32_t* ack = (int32_t*)cxt->ackbuf;
+  cxt->acklen  = 8;
+  return module->step_motor_read_pos_and_enc_pos(&ack[0], &ack[1]);
+}
 
 #undef MODULE_CLASS
 #define MODULE_CLASS ZIMiniServo
diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp
index b96d1f6..04e6bd5 100644
--- a/components/zcancmder/zcan_protocol_parser.hpp
+++ b/components/zcancmder/zcan_protocol_parser.hpp
@@ -77,6 +77,7 @@ class ZCanProtocolParser : public IZCanReceiverListener {
   CMDFN(step_motor_read_io_index_in_stm32);
   CMDFN(step_motor_set_subdevice_reg);
   CMDFN(step_motor_get_subdevice_reg);
+  CMDFN(step_motor_read_pos_and_enc_pos);
 
   CMDFN(mini_servo_enable);
   CMDFN(mini_servo_read_pos);