From c48e9293f0afddf48d57a69f6ce0b55850ca97a4 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Thu, 6 Jun 2024 09:18:42 +0800
Subject: [PATCH] update

---
 .../step_motor_ctrl_module.cpp                     | 29 ++++++++++++++++++++++
 1 file changed, 29 insertions(+)

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 306ff35..70e55b1 100644
--- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
+++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
@@ -311,6 +311,35 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio
 
   if (!check_when_run()) return false;
 
+  // 如果设备已经在零点，则反向移动一定距离远离零点
+  if (!gpio->getState()) {
+    ZLOGI(TAG, "---------STEP3--------  bak to gpio trigger edge");
+    _rotate(direction * lookPointVelocity, lookPointDec, lookPointDec);
+    bool reach_edge = false;
+
+    while (!m_thread.getExitFlag()) {
+      if (gpio->getState()) {
+        reach_edge = true;
+        m_stepM1->stop();
+        break;
+      }
+      vTaskDelay(1);
+    }
+
+    while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) {  // 等待电机停止
+      m_stepM1->stop();
+      vTaskDelay(100);
+    }
+
+    if (!reach_edge) {
+      ZLOGE(TAG, " bak to gpio trigger edge fail");
+      setErrorFlag(err::kstep_motor_not_found_point_edge, 0);
+      return false;
+    }
+  }
+
+  if (!check_when_run()) return false;
+
   if (m_thread.getExitFlag()) {
     ZLOGW(TAG, "break move to zero thread exit");
     return true;