update

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