From b1a8a3fb394d4fd2f3b752fef7391c6450fab800 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 16 Oct 2023 21:24:22 +0800 Subject: [PATCH 1/2] 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 From 9a71a1933196c65a2f25ffeca35740a3caf6169d Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 16 Oct 2023 21:28:15 +0800 Subject: [PATCH 2/2] fix some bug --- components/step_motor_ctrl_module/step_motor_ctrl_module.cpp | 3 +-- 1 file changed, 1 insertion(+), 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 dd736d8..870f79a 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -518,12 +518,10 @@ bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTar void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { // m_zero_shift_x x = motor_pos; - if (m_param.x_shaft != 0) x = -x; x += m_param.shift_x; } void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { x -= m_param.shift_x; - if (m_param.x_shaft != 0) x = -x; motor_pos = x; } #if 0 @@ -638,4 +636,5 @@ int32_t StepMotorCtrlModule::factory_reset() { void StepMotorCtrlModule::active_cfg() { m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay); m_stepM1->setScale(m_param.distance_scale); + m_stepM1->setMotorShaft(m_param.x_shaft); }