重构算法，现在能更加智能地判断是否需要前往释放点释放

TaiChi/TaiChi.ino

@@ -13,7 +13,7 @@ Servo servo; //舵机实例
 #define NORMAL_POINT 0 //普通点
 #define CATCH_POINT 1 //抓取点
 #define RELEASE_POINT 2 //释放点（使用机械臂）
-#define CARRY_POINT 0 //携带点，在算法中可视为普通点
+#define CARRY_POINT 3 //携带点（从底盘）
 #define GETOUT_POINT 4 //释放点（从底盘）
 
 //坐标点数组定义
@@ -94,10 +94,18 @@ bool is_claw_catch = false;
 
 //爪子是否正常
 bool is_claw_ok = true;
+
+//底盘是否携带环
+bool is_carry = false;
 //***************************************************************************************
 
 
 //****************************************自定函数****************************************
+#define NO_JUMP 0
+#define JUMP_DEAD_ROAD 1
+//更新标记
+void UpdateFlag(uint8_t jump_choice = NO_JUMP);
+
 //计算方向，同时更改完成转向后相对下一点的朝向
 uint8_t CalcDirection(void);
 
@@ -111,7 +119,7 @@ void LineForward(uint8_t end_position, float speed_rate = 1.0);
 //沿线后退，在触发条件后离开函数但不停止
 void LineBackward(uint8_t end_position, float speed_rate = 1.0);
 
-//直行或后退或转向，完成后离开函数但不停止
+//直行或后退或转向，完成后离开函数但不停止。会自动跳过无需前往的释放点
 void TurnDirection(float speed_rate = 1.0);
 
 //抓取环，并判定是否抓取成功
@@ -196,13 +204,27 @@ void loop()
     Serial.print("#TAICHI: is_claw_catch: "); Serial.print((int)is_claw_catch); Serial.print(" is_claw_ok: "); Serial.println((int)is_claw_ok);
     #endif
 
-    //情况一：刚完整经过普通点，下一个点为普通点
+    int8_t& passed_flag_type = route[passed_flag][TYPE];
-    if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == NORMAL_POINT)   
+    int8_t& next_flag_type = route[next_flag][TYPE];
+    int8_t& next_next_flag_type = route[next_next_flag][TYPE];
+
+    //情况一：刚完整经过普通点，下一个点为普通点或携带点
+    if (passed_flag_type == NORMAL_POINT && (next_flag_type == NORMAL_POINT || next_flag_type == CARRY_POINT))   
     {
         if (next_position == FRONT_NEXT)
         {
             //沿线直行，到前端传感器接触下一条线离开函数
             LineForward(FRONT_END);
+
+            //若下一点为携带点
+            if (next_flag_type == CARRY_POINT)
+            {
+                //底盘携带
+                is_carry = true;
+
+                //越过点成为普通点
+                next_flag_type = NORMAL_POINT;
+            }
         }
         else
         {
@@ -214,7 +236,7 @@ void loop()
         TurnDirection();
     }
     //情况二：刚完整经过普通点，下一个点为抓取点
-    else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == CATCH_POINT)
+    else if (passed_flag_type == NORMAL_POINT && next_flag_type == CATCH_POINT)
     {
         if (!is_claw_catch && !is_claw_ok) //爪子上无环且状态不正常
         {  
@@ -231,7 +253,12 @@ void loop()
             move.Stop();
 
             //抓取
-            CatchAndCheck();
+            if (!CatchAndCheck())
+                is_carry = true; //抓取失败
+        }
+        else //未进行抓取
+        {
+            is_carry = true;
         }
 
         //继续沿线直行，到前端传感器接触下一条线离开函数
@@ -241,10 +268,10 @@ void loop()
         TurnDirection();
 
         //将越过的点视为普通点。因为即使抓取失败，环也会被携带在底盘内
-        route[next_flag][TYPE] = NORMAL_POINT;
+        next_flag_type = NORMAL_POINT;
     }
     //情况三：刚完整经过普通点，下一个点为释放点（使用机械臂）
-    else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == RELEASE_POINT)
+    else if (passed_flag_type == NORMAL_POINT && next_flag_type == RELEASE_POINT)
     {
         //沿线直行，在释放位置离开函数
         LineForward(RELEASE_END);
@@ -252,19 +279,16 @@ void loop()
         //停止前进
         move.Stop();
 
-        if (is_claw_catch) //爪子有环
-        {   
         //释放
         servo.Release();
         delay(RELEASE_DELAY_TIME); //释放留时
         is_claw_catch = false;
-        }
 
         //下一点朝向为后
         next_position = BACK_NEXT;
     }
     //情况四：刚完整经过释放点（使用机械臂），下一个点为普通点
-    else if (route[passed_flag][TYPE] == RELEASE_POINT && route[next_flag][TYPE] == NORMAL_POINT)
+    else if (passed_flag_type == RELEASE_POINT && next_flag_type == NORMAL_POINT)
     {
         //沿线后退，到后端传感器接触下一条线离开函数
         LineBackward(BACK_END);
@@ -280,7 +304,7 @@ void loop()
         TurnDirection();
     }
     //情况五：刚完整经过普通点，下一个点为释放点（从底盘）
-    else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == GETOUT_POINT)
+    else if (passed_flag_type == NORMAL_POINT && next_flag_type == GETOUT_POINT)
     {
         //沿线直行，到后端传感器接触下一条线离开函数
         LineForward(BACK_END);
@@ -292,11 +316,13 @@ void loop()
         next_position = BACK_NEXT;
     }
     //情况六：刚完整经过释放点（从底盘），下一个点为普通点
-    else if (route[passed_flag][TYPE] == GETOUT_POINT && route[next_flag][TYPE] == NORMAL_POINT)
+    else if (passed_flag_type == GETOUT_POINT && next_flag_type == NORMAL_POINT)
     {
         //沿线后退，到前端传感器接触线离开函数
         LineBackward(FRONT_END);
 
+        is_carry = false;
+        
         //沿线后退，到后端传感器接触线离开函数
         LineBackward(BACK_END);
 
@@ -316,14 +342,7 @@ void loop()
     }
 
     //更新标记，继续循环
-    if (++passed_flag > max_flag)
+    UpdateFlag();
-        passed_flag = 0;
-
-    if (++next_flag > max_flag)
-        next_flag = 0;
-
-    if (++next_next_flag > max_flag)
-        next_next_flag = 0;
 
     #ifdef TAICHI_DEBUG
     Serial.println("#TAICHI: ====================End loop()====================");
@@ -331,6 +350,30 @@ void loop()
 }
 
 
+//更新标记
+void UpdateFlag(uint8_t jump_choice)
+{
+    if (jump_choice == NO_JUMP)
+    {
+        passed_flag = next_flag;
+
+        next_flag = next_next_flag;
+        
+        if (++next_next_flag > max_flag)
+            next_next_flag = 0;
+    }
+    else
+    {
+        int current_passed_flag = passed_flag;
+
+        UpdateFlag(NO_JUMP);
+        UpdateFlag(NO_JUMP);
+
+        passed_flag = current_passed_flag;
+    }
+}
+
+
 //计算方向，同时更改完成转向后相对下一点的朝向
 uint8_t CalcDirection(void)
 {
@@ -529,9 +572,20 @@ void LineBackward(uint8_t end_position, float speed_rate)
 }
 
 
-//直行或后退或转向，完成后离开函数但不停止
+//直行或后退或转向，完成后离开函数但不停止。会自动跳过无需前往的释放点
 void TurnDirection(float speed_rate)
 {    
+    //若下下点为释放点，判断是否需要跳过
+    if ((route[next_next_flag][TYPE] == RELEASE_POINT && (!is_claw_catch || is_carry)) || (route[next_next_flag][TYPE] == GETOUT_POINT && !is_carry))
+    {
+        UpdateFlag(JUMP_DEAD_ROAD);
+
+        #ifdef TAICHI_DEBUG
+        //调试输出直行或后退或转向状态
+        Serial.println("#TAICHI: JUMP THE RELEASE/GETOUT POINT FOR NOTHING CARRIED");
+         #endif
+    }
+    
     uint8_t direction = CalcDirection();
     
     #ifdef TAICHI_DEBUG