From 8180cfc8649b81e18ddca87862aeb47aae7ebfec Mon Sep 17 00:00:00 2001
From: "lxbpxylps@126.com" <lxbpxylps@126.com>
Date: Wed, 17 Feb 2021 19:35:43 +0800
Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=88=A4=E5=AE=9A=E6=98=AF?=
 =?UTF-8?q?=E5=90=A6=E6=8A=93=E5=8F=96=E6=88=90=E5=8A=9F=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 TaiChi/TaiChi.ino | 34 ++++++++++++++++++++++++++++++++--
 1 file changed, 32 insertions(+), 2 deletions(-)

diff --git a/TaiChi/TaiChi.ino b/TaiChi/TaiChi.ino
index 82d568b..5573d9c 100644
--- a/TaiChi/TaiChi.ino
+++ b/TaiChi/TaiChi.ino
@@ -114,6 +114,9 @@ void LineBackward(uint8_t end_position, float speed_rate = 1.0);
 
 //直行或后退或转向
 void TurnDirection(float speed_rate = 1.0);
+
+//抓取环，并判定是否抓取成功
+bool CatchAndCheckIsDone(float speed = 1.0);
 //***************************************************************************************
 
 
@@ -176,8 +179,7 @@ void loop()
         LineForward(CATCH_END);
 
         //抓取
-        servo.Catch();
-        delay(CATCH_DELAY_TIME); //抓取留时
+        CatchAndCheckIsDone();
 
         //继续沿线直行，到前端传感器接触下一条线为止
         LineForward(FRONT_END);
@@ -480,3 +482,31 @@ void TurnDirection(float speed_rate)
     Serial.println("#TAICHI: End Turn Direction");
     #endif
 }
+
+
+//抓取环，并判定是否抓取成功
+bool CatchAndCheckIsDone(float speed)
+{
+    //执行抓取动作
+    servo.Catch(speed);
+
+    //等待完成动作
+    delay(CATCH_DELAY_TIME);
+
+    if (sensor.IsPushed(BUTTON_1)) //开关 1 闭合，即爪子两端接触，说明抓取失败
+    {
+        servo.Catch(speed); //重试，此次检测但不重试
+        
+        //等待完成动作
+        delay(CATCH_DELAY_TIME);
+
+        if (sensor.IsPushed(BUTTON_1))
+        {
+            return false;
+        }
+    }
+    else
+    {
+        return true;
+    }
+}
\ No newline at end of file