hbot添加回零时丢步检测

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");

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();