fix some bug

components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -525,19 +525,20 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t
     x = m1pos + m2pos;
     y = m1pos - m2pos;
   }
-  x += m_basecfg.shift_x;
-  y += m_basecfg.shift_y;
 
   if (m_basecfg.x_shaft) x = -x;
   if (m_basecfg.y_shaft) y = -y;
+
+  x += m_basecfg.shift_x;
+  y += m_basecfg.shift_y;
 }
 void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& 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;
+
   if (m_basecfg.robot_type == corexy) {
     m1pos = ((x + y) / 2);
     m2pos = ((y - x) / 2);