From 76ef997ee4eea38d58ce29a67581e4318ca6f798 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 16 Oct 2023 15:48:40 +0800 Subject: [PATCH] fix xy motor bug --- components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index d03fccb..226f2f2 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -437,7 +437,6 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { ZLOGE(TAG, "find zero point fail"); return err::kce_y_leave_away_zero_point_fail; } - } if (m_Ygpio->getState()) { @@ -460,7 +459,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { return err::kce_y_leave_away_zero_point_fail; } } - + return 0; } @@ -525,11 +524,11 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t x = m1pos + m2pos; y = m1pos - m2pos; } - if (m_basecfg.x_shaft) x = -x; - if (m_basecfg.y_shaft) y = -y; - x += m_basecfg.shift_x; y += m_basecfg.shift_y; + + if (m_basecfg.x_shaft) x = -x; + if (m_basecfg.y_shaft) y = -y; } void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { if (m_basecfg.x_shaft) x = -x;