update

sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -251,6 +251,19 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to_zero() {
   return 0;
 }
 
+int32_t XYRobotCtrlModule::waiting_for_stop(function<bool()> condition_fn, int32_t delay) {
+  while (!_motor_is_reach_target()) {
+    if (m_thread.getExitFlag()) break;
+    if (condition_fn()) break;
+    osDelay(delay);
+  }
+  _motor_stop();
+  while (_motor_is_reach_target()) {
+    osDelay(10);
+  }
+  return 0;
+}
+
 int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
   /*******************************************************************************
    *                                  移动导零点                                   *
@@ -261,51 +274,15 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
 
   if (!xgpio->getState()) {
     _motor_move_to_end(-1, 0, m_cfg.run_to_zero_speed);
-    bool xreach_zero = false;
-
-    while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
-      // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
-      if (xgpio->getState()) {
-        xreach_zero = true;
-        _motor_stop();
-        break;
-      }
-      vTaskDelay(1);
-    }
-
-    if (m_thread.getExitFlag()) {
-      ZLOGI(TAG, "break move to zero thread exit");
-      return err::kmodule_opeation_break_by_user;
-    }
-
-    if (!xreach_zero) {
-      ZLOGE(TAG, "find zero point fail");
-      return err::kxymotor_x_find_zero_edge_fail;
-    }
+    waiting_for_stop([this, xgpio]() { return xgpio->getState(); }, 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);
-    bool xleave_zero = false;
-    while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
-      if (!xgpio->getState()) {
-        xleave_zero = true;
-        _motor_stop();
-        break;
-      }
-      vTaskDelay(1);
-    }
-
-    if (m_thread.getExitFlag()) {
-      ZLOGI(TAG, "break move to zero thread exit");
-      return err::kmodule_opeation_break_by_user;
-    }
-
-    if (!xleave_zero) {
-      ZLOGI(TAG, "leave away zero failed");
-      return err::kxymotor_x_find_zero_edge_fail;
-    }
+    waiting_for_stop([this, xgpio]() { return !xgpio->getState(); }, 2);
+    if (m_thread.getExitFlag()) return err::kmodule_opeation_break_by_user;
   }
   ZLOGI(TAG, "step2 leave x zero            ok");
   /*******************************************************************************
@@ -314,47 +291,18 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
 
   if (!ygpio->getState()) {
     _motor_move_to_end(0, -1, m_cfg.run_to_zero_speed);
-    bool yreach_zero = false;
-    while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
-      if (ygpio->getState()) {
-        yreach_zero = true;
-        _motor_stop();
-        break;
-      }
-      vTaskDelay(1);
-    }
-    if (m_thread.getExitFlag()) {
-      ZLOGI(TAG, "break move to zero thread exit");
-      return err::kmodule_opeation_break_by_user;
-    }
-    if (!yreach_zero) {
-      ZLOGE(TAG, "find zero point fail");
-      return err::kxymotor_y_find_zero_edge_fail;
-    }
+    waiting_for_stop([this, ygpio]() { return ygpio->getState(); }, 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);
-    bool yleave_zero = false;
-    while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
-      if (!ygpio->getState()) {
-        yleave_zero = true;
-        _motor_stop();
-        break;
-      }
-      vTaskDelay(1);
-    }
-    if (m_thread.getExitFlag()) {
-      ZLOGI(TAG, "break move to zero thread exit");
-      return err::kmodule_opeation_break_by_user;
-    }
-    if (!yleave_zero) {
-      ZLOGI(TAG, "leave away zero failed");
-      return err::kxymotor_y_find_zero_edge_fail;
-    }
+    waiting_for_stop([this, ygpio]() { return !ygpio->getState(); }, 2);
+    if (m_thread.getExitFlag()) return err::kmodule_opeation_break_by_user;
   }
   ZLOGI(TAG, "step4 leave y zero            ok");
+
   ZLOGI(TAG, "move to zero ok");
   return 0;
 }

sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp

@@ -108,6 +108,9 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule {
   virtual int32_t xymotor_motor_move_by_direct(int32_t motor1_dpos, int32_t motor2_dpos) override;
   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);
+
  private:
   int32_t do_public_check();
   int32_t do_xymotor_move_to(int32_t x, int32_t y);

usrc/subboards/subboard10_hbot_v2/subboard10_hbot_v2.cpp

@@ -90,13 +90,13 @@ void Subboard10HbotV2::initialize() {
   static XYRobotCtrlModule::config_t xy_defaultcfg;
   XYRobotCtrlModule::create_default_cfg(xy_defaultcfg);
 
-#define LEVEL1_A         10
-#define LEVEL2_A         30
+#define LEVEL1_A         5
+#define LEVEL2_A         15
 #define START_AND_STOP_V 30
 #define V1               200
 
   xy_defaultcfg.robot_type                   = XYRobotCtrlModule::khbot;
-  xy_defaultcfg.one_circle_pulse             = 7215;
+  xy_defaultcfg.one_circle_pulse             = 7200;
   xy_defaultcfg.one_circle_pulse_denominator = 10;
   xy_defaultcfg.ihold                        = 10;
   xy_defaultcfg.irun                         = 31;
@@ -122,7 +122,7 @@ void Subboard10HbotV2::initialize() {
   xy_defaultcfg.max_x                        = 5714;
   xy_defaultcfg.max_y                        = 4158;
   xy_defaultcfg.run_to_zero_speed            = 80;
-  xy_defaultcfg.look_zero_edge_speed         = 10;
+  xy_defaultcfg.look_zero_edge_speed         = 2;
 
   xyRobotCtrlModule.initialize(getmoduleId(1), &motora, &motorb, &input[0], 4, xy_defaultcfg);