From 40531be4a6a1d8479adeaaa83fee988224d4f393 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Sun, 9 Jun 2024 12:37:37 +0800
Subject: [PATCH] update

---
 .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp  | 41 ++++++++++++++++++----
 1 file changed, 34 insertions(+), 7 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 93a4bf2..3f44c18 100644
--- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
+++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
@@ -173,7 +173,7 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to(int32_t x, int32_t y) {
           m_stepM2->setAcceleration(m_cfg.acc);
           m_stepM2->setDeceleration(m_cfg.dec);
 
-          // ZLOGI(TAG, "_motor_move_to target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos);
+          ZLOGI(TAG, "_motor_move_to target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos);
           m_stepM1->moveTo(target_m1pos, m_cfg.default_velocity);
           m_stepM2->moveTo(target_m2pos, m_cfg.default_velocity);
 
@@ -200,8 +200,13 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to_zero() {
 
   m_thread.start([this]() {
     befor_motor_move();
-    exec_move_to_zero_task();
+    int32_t ecode = exec_move_to_zero_task();
     after_motor_move();
+    if (ecode == 0) {
+      m_stepM1->setXACTUAL(0);
+      m_stepM2->setXACTUAL(0);
+    }
+
     return;
   });
   return 0;
@@ -316,12 +321,34 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
 }
 
 void XYRobotCtrlModule::_motor_move_to_end(int32_t xdirection, int32_t ydirection, int32_t maxv) {
-  ZLOGI(TAG, "_motor_move_to_end xdirection:%d ydirection:%d maxv:%d acc:%d dec:%d ", xdirection, ydirection, maxv);
-  if (xdirection != 0) {
-    m_stepM1->rotate(xdirection * maxv);
-  } else if (ydirection != 0) {
-    m_stepM2->rotate(ydirection * maxv);
+  ZLOGI(TAG, "_motor_move_to_end xdirection:%d ydirection:%d maxv:%d ", xdirection, ydirection, maxv);
+
+  int32_t dx = 0, dy = 0;
+  if (xdirection > 0) {
+    dx = 100000;
+  } else if (xdirection < 0) {
+    dx = -100000;
+  } else {
+    dx = 0;
   }
+
+  if (ydirection > 0) {
+    dy = 100000;
+  } else if (ydirection < 0) {
+    dy = -100000;
+  } else {
+    dy = 0;
+  }
+
+  int32_t t_x, t_y;
+  getnowpos(t_x, t_y);
+  t_x = t_x + dx;
+  t_y = t_y + dy;
+
+  int32_t target_m1pos, target_m2pos;
+  forward_kinematics(t_x, t_y, target_m1pos, target_m2pos);
+  m_stepM1->moveTo(target_m1pos, maxv);
+  m_stepM2->moveTo(target_m2pos, maxv);
 }
 bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); }
 void XYRobotCtrlModule::_motor_stop() {