From b1a8a3fb394d4fd2f3b752fef7391c6450fab800 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Mon, 16 Oct 2023 21:24:22 +0800
Subject: [PATCH] update

---
 components/step_motor_ctrl_module/step_motor_ctrl_module.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

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 331174b..dd736d8 100644
--- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
+++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
@@ -483,7 +483,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
 
 void StepMotorCtrlModule::getnowpos(int32_t& pos) {
   int32_t motor_pos = m_stepM1->getXACTUAL();
-  forward_kinematics(motor_pos, pos);
+  inverse_kinematics(motor_pos, pos);
 }
 void StepMotorCtrlModule::_motor_move_to(int32_t pos, int32_t maxv, int32_t acc, int32_t dec) {
   ZLOGI(TAG, "m%d _motor_move_to %d maxv:%d acc:%d dec:%d", m_id, pos, maxv, acc, dec);
@@ -522,8 +522,8 @@ void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) {
   x += m_param.shift_x;
 }
 void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) {
-  if (m_param.x_shaft != 0) x = -x;
   x -= m_param.shift_x;
+  if (m_param.x_shaft != 0) x = -x;
   motor_pos = x;
 }
 #if 0