From 6f91871427943f931cdc5c2142e742c771077fd2 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Mon, 29 Jul 2024 17:29:07 +0800
Subject: [PATCH] =?UTF-8?q?hbot=E6=B7=BB=E5=8A=A0=E5=9B=9E=E9=9B=B6?=
 =?UTF-8?q?=E6=97=B6=E4=B8=A2=E6=AD=A5=E6=A3=80=E6=B5=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp           | 13 ++++++++-----
 .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp           |  2 +-
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
index 7dac194..c6d2a9f 100644
--- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
+++ b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
@@ -251,10 +251,13 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to_zero() {
   return 0;
 }
 
-int32_t XYRobotCtrlModule::waiting_for_stop(function<bool()> condition_fn, int32_t delay) {
+int32_t XYRobotCtrlModule::waiting_for_stop(function<bool()> condition_fn, bool checkWhenLoop, int32_t delay) {
   while (!_motor_is_reach_target()) {
     if (m_thread.getExitFlag()) break;
     if (condition_fn()) break;
+    if (checkWhenLoop && !check_when_run()) {
+      break;
+    }
     osDelay(delay);
   }
   _motor_stop();
@@ -274,14 +277,14 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
 
   if (!xgpio->getState()) {
     _motor_move_to_end(-1, 0, m_cfg.run_to_zero_speed);
-    waiting_for_stop([this, xgpio]() { return xgpio->getState(); }, 2);
+    waiting_for_stop([this, xgpio]() { return xgpio->getState(); }, true, 2);
     if (m_thread.getExitFlag()) return err::kmodule_opeation_break_by_user;
   }
   ZLOGI(TAG, "step1 reach x zero             ok");
 
   if (xgpio->getState()) {
     _motor_move_to_end(1, 0, m_cfg.look_zero_edge_speed);
-    waiting_for_stop([this, xgpio]() { return !xgpio->getState(); }, 2);
+    waiting_for_stop([this, xgpio]() { return !xgpio->getState(); }, false, 2);
     if (m_thread.getExitFlag()) return err::kmodule_opeation_break_by_user;
   }
   ZLOGI(TAG, "step2 leave x zero            ok");
@@ -291,14 +294,14 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
 
   if (!ygpio->getState()) {
     _motor_move_to_end(0, -1, m_cfg.run_to_zero_speed);
-    waiting_for_stop([this, ygpio]() { return ygpio->getState(); }, 2);
+    waiting_for_stop([this, ygpio]() { return ygpio->getState(); }, true, 2);
     if (m_thread.getExitFlag()) return err::kmodule_opeation_break_by_user;
   }
   ZLOGI(TAG, "step3 reach y zero            ok");
 
   if (ygpio->getState()) {
     _motor_move_to_end(0, 1, m_cfg.look_zero_edge_speed);
-    waiting_for_stop([this, ygpio]() { return !ygpio->getState(); }, 2);
+    waiting_for_stop([this, ygpio]() { return !ygpio->getState(); }, false, 2);
     if (m_thread.getExitFlag()) return err::kmodule_opeation_break_by_user;
   }
   ZLOGI(TAG, "step4 leave y zero            ok");
diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
index a8c7524..d276527 100644
--- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
+++ b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
@@ -109,7 +109,7 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule {
   virtual int32_t xymotor_read_enc_direct(int32_t* enc1, int32_t* enc2) override;
 
  public:
-  virtual int32_t waiting_for_stop(function<bool()> condition_fn, int32_t delay);
+  virtual int32_t waiting_for_stop(function<bool()> condition_fn,bool checkWhenLoop, int32_t delay);
 
  private:
   int32_t do_public_check();