From 872f35613e3f61f5e48ee8d9aefc7d862261849d Mon Sep 17 00:00:00 2001
From: Glz <354338225@qq.com>
Date: Sat, 23 Mar 2024 17:37:31 +0800
Subject: [PATCH] =?UTF-8?q?=E9=81=BF=E9=9A=9C=E5=B7=A1=E7=BA=BF=E6=9B=B4?=
 =?UTF-8?q?=E6=94=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/gl_barrier.c  | 22 ++++++++++++++++++++++
 app/gl_barrier.h  |  1 +
 app/gl_tracking.c | 45 ++++++++++++++++++++++++++++-----------------
 3 files changed, 51 insertions(+), 17 deletions(-)

diff --git a/app/gl_barrier.c b/app/gl_barrier.c
index d5b0e11..c9992bc 100644
--- a/app/gl_barrier.c
+++ b/app/gl_barrier.c
@@ -13,3 +13,25 @@ void CheckBarrier() {
     }
 }
 
+void RunBarrier() {
+  if (barrier_type == BARRIER_LEFT_BEGIN) // 左环开始，寻外直道右线
+  {
+    track_type = TRACK_RIGHT;
+  }
+  else if (barrier_type == BARRIER_LEFT_BEGIN && ((Lpt0_found_barrier && is_straight1) || Lpt0_found) )
+  {
+   barrier_type = BARRIER_NONE;
+   track_type = TRACK_RIGHT;
+  }
+
+  if (barrier_type == BARRIER_RIGHT_BEGIN) // 左环开始，寻外直道右线
+  {
+    track_type = TRACK_LEFT;
+  }
+  else if (barrier_type == BARRIER_RIGHT_BEGIN && ((Lpt1_found_barrier && is_straight0) || Lpt1_found) )
+  {
+   barrier_type = BARRIER_NONE;
+   track_type = TRACK_LEFT;
+  }
+ 
+}
diff --git a/app/gl_barrier.h b/app/gl_barrier.h
index edd73ba..86131c2 100644
--- a/app/gl_barrier.h
+++ b/app/gl_barrier.h
@@ -14,6 +14,7 @@ enum barrier_type_e {
 extern enum barrier_type_e barrier_type;
 
 void CheckBarrier(void);
+void RunBarrier(void);
 
 
 #endif /* COMMON_H_ */
\ No newline at end of file
diff --git a/app/gl_tracking.c b/app/gl_tracking.c
index b4ff355..edccd7e 100644
--- a/app/gl_tracking.c
+++ b/app/gl_tracking.c
@@ -28,6 +28,8 @@ void aim_distance_select(void)
         aim_distance = 0.49f;
     } else if (circle_type != CIRCLE_NONE) {
         aim_distance = 0.2f;
+    } else if (barrier_type != BARRIER_NONE) {
+        aim_distance = 0.3f;
     } else {
         aim_distance = COMMON_AIM;
     }
@@ -40,7 +42,7 @@ void ElementJudge()
         CheckCross();
         if (cross_type == CROSS_NONE) {
             CheckBarrier();
-            if (barrier_type == BARRIER_NONE){
+            if (barrier_type == BARRIER_NONE) {
                 CheckCircle();
             }
         }
@@ -50,17 +52,13 @@ void ElementJudge()
         cross_type  = CROSS_NONE;
         circle_type = CIRCLE_NONE;
     }
-    if (cross_type != CROSS_NONE)
-    {
-        circle_type = CIRCLE_NONE;
+    if (cross_type != CROSS_NONE) {
+        circle_type  = CIRCLE_NONE;
         barrier_type = BARRIER_NONE;
     }
-    if (barrier_type != BARRIER_NONE)
-    {
+    if (barrier_type != BARRIER_NONE) {
         circle_type = CIRCLE_NONE;
     }
-    
-    
 }
 
 void ElementRun()
@@ -75,6 +73,8 @@ void ElementRun()
 
     else if (circle_type != CIRCLE_NONE) {
         RunCircle();
+    } else if (barrier_type != BARRIER_NONE) {
+        RunBarrier();
     }
 }
 
@@ -137,15 +137,26 @@ void MidLineTrack()
         float dn = (float)Q_sqrt(dx * dx + dy * dy);
         //  float error = -atan2f(dx, dy) * 180 / PI32;
 
-        // 计算近锚点偏差值
-        dx_near = rptsn[aim_idx_near][1] - cx;
-        // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
-        // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
-        // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
+        if (barrier_type == BARRIER_LEFT_BEGIN) {
+            dx_near    = rptsn[aim_idx_near][1] - cx;
+            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
+        } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
+            dx_near    = rptsn[aim_idx_near][1] - cx;
+            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
+        } else {
+            dx_near    = rptsn[aim_idx_near][1] - cx;
+            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
+        }
 
-        // //考虑近点
-        // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ;
-        // //考虑远点
-        pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
+        // // 计算近锚点偏差值
+        // dx_near = rptsn[aim_idx_near][1] - cx;
+        // // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
+        // // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
+        // // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
+
+        // // //考虑近点
+        // // near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI32 * 180 ;
+        // // //考虑远点
+        // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f;
     }
 }