Merge branch 'master' of https://git.brisky.space/btl143/QDAC-firmware

feat:增加cross处理开关

app/by_frame.c

@@ -7,8 +7,8 @@
 #include "lwrb.h"
 #include "crc16.h"
 
-uint8_t frame_buffer_recv[(2 * (4 + BY_FRAME_DATA_NUM * sizeof(uint32_t))) + 1];
+uint8_t frame_buffer_recv[(2 * (4 + BY_FRAME_DATA_NUM_RECV * sizeof(uint32_t))) + 1];
-uint8_t frame_buffer_send[4 + BY_FRAME_DATA_NUM * sizeof(uint32_t)];
+uint8_t frame_buffer_send[4 + BY_FRAME_DATA_NUM_SEND * sizeof(uint32_t)];
 uint8_t frame_parse_busy;
 lwrb_t lwrb_ctx;
 
@@ -19,10 +19,10 @@ void by_frame_init(void)
     uart_rx_interrupt(BY_FRAME_UART_INDEX, ENABLE);
 }
 
-void by_frame_send(uint32_t *data_array)
+void by_frame_send(uint32_t* data_array)
 {
     uint16_t crc_cal            = 0;
-    const uint8_t data_byte_num = BY_FRAME_DATA_NUM * sizeof(uint32_t);
+    const uint8_t data_byte_num = BY_FRAME_DATA_NUM_SEND * sizeof(uint32_t);
 
     frame_buffer_send[0] = BY_FRAME_HEAD_1;
     frame_buffer_send[1] = BY_FRAME_HEAD_2;
@@ -46,12 +46,12 @@ void by_frame_send(uint32_t *data_array)
  */
 void by_frame_parse(uint32_t *data_array)
 {
-    uint32_t len                                                = lwrb_get_full(&lwrb_ctx); // 缓冲区大小
+    uint32_t len                = lwrb_get_full(&lwrb_ctx); // 缓冲区大小
-    uint8_t status                                              = 0;                        // 状态 0-未找到帧头 1-找到帧头 2-校验
+    uint8_t status              = 0;                        // 状态 0-未找到帧头 1-找到帧头 2-校验
-    uint16_t frame_start                                        = 0;                        // 帧起始位置
+    uint16_t frame_start        = 0;                        // 帧起始位置
-    uint8_t frame_buf[4 + BY_FRAME_DATA_NUM * sizeof(uint32_t)] = {0};                      // 帧
+    uint8_t frame_buf[4 + BY_FRAME_DATA_NUM_RECV * sizeof(uint32_t)]    = {0};                      // 帧
-    uint8_t buf[(4 + BY_FRAME_DATA_NUM * sizeof(uint32_t)) * 2] = {0};                      // 用于解析的数据块
+    uint8_t buf[(4 + BY_FRAME_DATA_NUM_RECV * sizeof(uint32_t)) * 2]    = {0};                      // 用于解析的数据块
-    const uint8_t data_byte_num                                 = BY_FRAME_DATA_NUM * sizeof(uint32_t);
+    const uint8_t data_byte_num = BY_FRAME_DATA_NUM_RECV * sizeof(uint32_t);
 
     if (len < 2 * (4 + data_byte_num)) {
         // 当前要求缓冲区满

app/by_frame.h

@@ -8,20 +8,20 @@
 
 #include <stdint.h>
 
-#define BY_FRAME_HEAD_1            (0XEB)
+#define BY_FRAME_HEAD_1        (0XEB)
-#define BY_FRAME_HEAD_2            (0x90)
+#define BY_FRAME_HEAD_2        (0x90)
 
-#define BY_FRAME_UART_TX_PIN       (UART2_MAP0_TX_A2)
+#define BY_FRAME_UART_TX_PIN   (UART2_MAP0_TX_A2)
-#define BY_FRAME_UART_RX_PIN       (UART2_MAP0_RX_A3)
+#define BY_FRAME_UART_RX_PIN   (UART2_MAP0_RX_A3)
-#define BY_FRAME_UART_INDEX        (UART_2)
+#define BY_FRAME_UART_INDEX    (UART_2)
-#define BY_FRAME_UART_BAUDRATE     (115200)
+#define BY_FRAME_UART_BAUDRATE (115200)
 
-#define BY_FRAME_DATA_NUM      (3)
+#define BY_FRAME_DATA_NUM_SEND (3)
+#define BY_FRAME_DATA_NUM_RECV (1)
 
 extern void by_frame_init(void);
 void by_frame_send(uint32_t *data_array);
 void by_frame_parse(uint32_t *data_array);
 extern void by_frame_parse_uart_handle(uint8_t data);
 
-
 #endif

app/gl_barrier.c

@@ -3,13 +3,14 @@
 
 enum barrier_type_e barrier_type = BARRIER_NONE;
 enum s_type_e s_type             = S_NONE;
-uint16 time_barrier;
+enum shield_type_e shield_type   = SHIELD_NONE;
 float (*mid_track_s)[2];
 int32_t mid_track_count_s;
 int temp_min_s;
 int temp_max_s;
 int barrier_begin_flag = 0;
-
+int barrier_count      = 0;
+float barrirer_time=500.f;
 void CheckBarrier()
 {
     if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) {
@@ -22,91 +23,170 @@ void CheckBarrier()
 
 void RunBarrier()
 {
-    if (barrier_type == BARRIER_LEFT_BEGIN) {
-        track_type = TRACK_RIGHT;
-        if (barrier_begin_flag == 0) {
-            timer_clear(TIM_3);
-            timer_start(TIM_3);
-            barrier_begin_flag = 1;
-        }
-    }
     if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
-        if (barrier_begin_flag == 1) {
+        track_type = TRACK_RIGHT;
-            barrier_type = BARRIER_LEFT_BEGIN;
+        if (barrier_type == BARRIER_LEFT_BEGIN)
-            time_barrier = timer_get(TIM_3);
+        {
-            if (time_barrier >= 300 && time_barrier < 600) {
+            timer_clear(TIM_2);
+            timer_start(TIM_2);
+            barrier_type = BARRIER_LEFT_RUNNING;
+        }
+        if (barrier_type == BARRIER_LEFT_RUNNING)
+        {
+            if (timer_get(TIM_2) >= barrirer_time)
+            {
+                barrier_type = BARRIER_NONE;
+                timer_stop(TIM_2);
+                timer_clear(TIM_2);
+            }
+            else{
                 barrier_type = BARRIER_LEFT_RUNNING;
-            } else if (time_barrier >= 600) {
-                barrier_type       = BARRIER_NONE;
-                barrier_begin_flag = 0;
-                timer_stop(TIM_3); 
-                timer_clear(TIM_3);
             }
         }
+        
+        
+        //     if (barrier_begin_flag == 0) {
+        //         timer_clear(TIM_3);
+        //         timer_start(TIM_3);
+        //         barrier_begin_flag = 1;
+        //     }
+        // }
+        // if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
+        //     if (barrier_begin_flag == 1) {
+        //         barrier_type = BARRIER_LEFT_BEGIN;
+        //         time_barrier = timer_get(TIM_3);
+        //         if (time_barrier >= 300 && time_barrier < 600) {
+        //             barrier_type = BARRIER_LEFT_RUNNING;
+        //         } else if (time_barrier >= 600) {
+        //             barrier_type       = BARRIER_NONE;
+        //             barrier_count = 1;
+        //             barrier_begin_flag = 0;
+        //             timer_stop(TIM_3);
+        //             timer_clear(TIM_3);
+        //         }
+        //     }
+        // }
+        // if (pts_resample_left_count >= 5) {
+        //     if (Lpt0_found) {
+        //         Lpt0_found_count++;
+        //     }
+        //     if (Lpt0_found_count >= 1 && barrier_type == BARRIER_LEFT_BEGIN) {
+        //         Lpt0_found_count = 0;
+        //         barrier_type     = BARRIER_LEFT_RUNNING;
+        //         track_type       = TRACK_RIGHT;
+        //         timer_clear(TIM_2);
+        //         timer_start(TIM_2);
+        //     } else if (barrier_type == BARRIER_LEFT_RUNNING) {
+        //         track_type = TRACK_RIGHT;
+        //         if (timer_get(TIM_2) >= 500) {
+        //             barrier_type = BARRIER_NONE;
+        //             track_type   = TRACK_RIGHT;
+        //             timer_stop(TIM_2);
+        //             timer_clear(TIM_2);
+        //         }
+        //     }
+        // } else {
+        //     track_type   = TRACK_RIGHT;
+        //     barrier_type = BARRIER_NONE;
+        // }
+
+        // if (barrier_type == BARRIER_LEFT_RUNNING) {
+        //     if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) {
+        //         barrier_type = BARRIER_NONE;
+        //     }
+        // }
+
+        // if (Lpt0_found) {
+        //     Lpt0_found_count++;
+        // }
+        // if (Lpt0_found_count >= 1 && barrier_type == BARRIER_LEFT_BEGIN) {
+        //     Lpt0_found_count = 0;
+        //     barrier_type     = BARRIER_LEFT_RUNNING;
+        //     track_type       = TRACK_RIGHT;
+        //     timer_clear(TIM_2);
+        //     timer_start(TIM_2);
+        // } else if (barrier_type == BARRIER_LEFT_RUNNING) {
+        //     track_type   = TRACK_RIGHT;
+        //     if (timer_get(TIM_2) >= 500) {
+        //         barrier_type = BARRIER_NONE;
+        //         track_type   = TRACK_RIGHT;
+        //         timer_stop(TIM_2);
+        //         timer_clear(TIM_2);
+        //     }
+        // }
     }
 
-    //     if (Lpt0_found) {
-    //         Lpt0_found_count++;
-    //     }
-    //     if (Lpt0_found_count >= 1) {
-    //         Lpt0_found_count = 0;
-    //         barrier_type     = BARRIER_LEFT_RUNNING;
-    //         track_type       = TRACK_RIGHT;
-    //         timer_clear(TIM_3);
-    //         timer_start(TIM_3);
-    //     }
-    // } else if (barrier_type == BARRIER_LEFT_RUNNING) {
-    //     track_type   = TRACK_RIGHT;
-    //     time_barrier = timer_get(TIM_3);
-    //     if (time_barrier >= 220) {
-    //         barrier_type = BARRIER_NONE;
-    //         track_type   = TRACK_RIGHT;
-    //         timer_start(TIM_3);
-    //         timer_clear(TIM_3);
-    //     }
-
-    if (barrier_type == BARRIER_RIGHT_BEGIN) {
-        track_type = TRACK_LEFT;
-        if (barrier_begin_flag == 0) {
-            timer_clear(TIM_3);
-            timer_start(TIM_3);
-            barrier_begin_flag = 1;
-        }
-    }
     if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
-        if (barrier_begin_flag == 1) {
+        track_type = TRACK_LEFT;
-            barrier_type = BARRIER_RIGHT_BEGIN;
+        if (barrier_type == BARRIER_RIGHT_BEGIN)
-            time_barrier = timer_get(TIM_3);
+        {
-            if (time_barrier >= 300 && time_barrier < 600) {
+            timer_clear(TIM_2);
+            timer_start(TIM_2);
+            barrier_type = BARRIER_RIGHT_RUNNING;
+        }
+        if (barrier_type == BARRIER_RIGHT_RUNNING)
+        {
+            if (timer_get(TIM_2) >= barrirer_time)
+            {
+                barrier_type = BARRIER_NONE;
+                timer_stop(TIM_2);
+                timer_clear(TIM_2);
+            }
+            else{
                 barrier_type = BARRIER_RIGHT_RUNNING;
-            } else if (time_barrier >= 600) {
-                barrier_type       = BARRIER_NONE;
-                barrier_begin_flag = 0;
-                timer_stop(TIM_3);
-                timer_clear(TIM_3);
             }
         }
-    }
+        //     if (barrier_begin_flag == 0) {
+        //         timer_clear(TIM_3);
+        //         timer_start(TIM_3);
+        //         barrier_begin_flag = 1;
+        //     }
+        // }
+        // if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
+        //     if (barrier_begin_flag == 1) {
+        //         barrier_type = BARRIER_RIGHT_BEGIN;
+        //         time_barrier = timer_get(TIM_3);
+        //         if (time_barrier >= 300 && time_barrier < 600) {
+        //             barrier_type = BARRIER_RIGHT_RUNNING;
+        //         } else if (time_barrier >= 600) {
+        //             barrier_type       = BARRIER_NONE;
+        //             barrier_count = 1;
+        //             barrier_begin_flag = 0;
+        //             timer_stop(TIM_3);
+        //             timer_clear(TIM_3);
+        //         }
+        //     }
+        // }
+        // if (pts_resample_right_count >= 5) {
+        //     if (Lpt1_found) {
+        //         Lpt1_found_count++;
+        //     }
+        //     if (Lpt1_found_count >= 1 && barrier_type == BARRIER_RIGHT_BEGIN) {
+        //         Lpt1_found_count = 0;
+        //         barrier_type     = BARRIER_RIGHT_RUNNING;
+        //         track_type       = TRACK_LEFT;
+        //         timer_clear(TIM_2);
+        //         timer_start(TIM_2);
+        //     } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
+        //         track_type = TRACK_LEFT;
+        //         if (timer_get(TIM_2) >= 500) {
+        //             timer_stop(TIM_2);
+        //             timer_clear(TIM_2);
+        //             barrier_type = BARRIER_NONE;
+        //             track_type   = TRACK_LEFT;
+        //         }
+        //     }
+        // } else {
+        //     track_type   = TRACK_LEFT;
+        //     barrier_type = BARRIER_NONE;
+        // }
 
-    //     if (Lpt1_found) {
+        // if (barrier_type == BARRIER_RIGHT_RUNNING) {
-    //         Lpt1_found_count++;
+        //     if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) {
-    //     }
+        //         barrier_type = BARRIER_NONE;
-    //     if (Lpt1_found_count >= 1) {
+        //     }
-    //         Lpt1_found_count = 0;
+        // }
-    //         barrier_type     = BARRIER_RIGHT_RUNNING;
+    }
-    //         track_type       = TRACK_LEFT;
-    //         timer_clear(TIM_3);
-    //         timer_start(TIM_3);
-    //     }
-    // } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
-    //     track_type   = TRACK_LEFT;
-    //     time_barrier = timer_get(TIM_3);
-    //     if (time_barrier >= 220) {
-    //         timer_start(TIM_3);
-    //         timer_clear(TIM_3);
-    //         barrier_type = BARRIER_NONE;
-    //         track_type   = TRACK_LEFT;
-    //     }
 }
 
 void Check_s()
@@ -144,4 +224,21 @@ void RunS()
     if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) {
         s_type = S_NONE;
     }
-}
+}
+
+// void Check_shield(){
+//     if (begin_flag == 1)
+//     {
+//         shield_type = SHIELD_BEGIN;
+//         timer_clear(TIM_3);
+//         timer_start(TIM_3);
+//         begin_flag = 0;
+//     }
+//     if (timer_get(TIM_3) > 1000 && shield_type == SHIELD_BEGIN)
+//     {
+//         shield_type = SHIELD_NONE;
+//         timer_stop(TIM_3);
+//         timer_clear(TIM_3);
+//     }
+
+// }

app/gl_barrier.h

@@ -18,17 +18,23 @@ enum s_type_e {
   S_OUT,
 };
 
-extern uint16 time_barrier;
+enum shield_type_e {
+  SHIELD_NONE,
+  SHIELD_BEGIN,
+};
+
 extern enum barrier_type_e barrier_type;
 extern enum s_type_e s_type;
+extern enum shield_type_e shield_type;
 extern float (*mid_track_s)[2];
 extern int32_t mid_track_count_s;
 extern int temp_min_s;
 extern int temp_max_s;
-
+extern float barrirer_time;
 void CheckBarrier(void);
 void Check_s(void);
 void RunBarrier(void);
+void Check_shield(void);
 
 
 #endif /* COMMON_H_ */

app/gl_circle.c

@@ -15,6 +15,8 @@ int32_t Right_Border_ToLeft_Circle = 0;
 int32_t Left_Border_ToRight_Circle  = 0;
 int32_t Right_Border_ToRight_Circle = 0;
 
+int32_t ceshi = 0;
+
 void CheckCircle()
 {
     // 非圆环模式下，单边L角点, 单边长直道
@@ -59,7 +61,7 @@ void RunCircle()
         }
     } else if (circle_type == CIRCLE_LEFT_RUNNING) // 正常巡线，寻外圆右线
     {
-        //track_type = TRACK_RIGHT;
+        // track_type = TRACK_RIGHT;
         track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
         if (Lpt1_found) {
             pts_resample_right_count = mid_right_count = Lpt1_rpts1s_id;
@@ -125,8 +127,8 @@ void RunCircle()
     } else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线
     {
         track_type = TRACK_RIGHT;
-        //track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
+        // track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
-        if (Lpt0_found)          // 外环存在拐点，可再加拐点距离判据 (左 L 点)
+        if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点)
         {
             pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id;
         }
@@ -149,13 +151,13 @@ void RunCircle()
         {
             Right_Border_None_Circle++;
         }
-        if (pts_resample_right_count > 0.7 / RESAMPLEDIST &&
+        if (pts_resample_right_count > (0.7 / RESAMPLEDIST) && Right_Border_None_Circle > FRAMENONE) {
-            Right_Border_None_Circle > FRAMENONE) {
             circle_type                = CIRCLE_NONE;
             Right_Border_None_Circle   = 0;
             Right_Border_Have_Circle   = 0;
             Left_Border_ToLeft_Circle  = 0;
             Left_Border_ToRight_Circle = 0;
         }
+
     }
 }

app/gl_circle.h

@@ -17,6 +17,7 @@ enum circle_type_e {
 };
 
 extern enum circle_type_e circle_type;
+extern int32_t ceshi;
 
 void CheckCircle(void);
 void RunCircle(void);

app/gl_common.h

@@ -16,6 +16,7 @@
 #define SELFADAPT_OFFSET      (8)                          // 适应性块大小
 #define PIXPERMETER           (70.0f)
 #define PIXPERMETER_ACROSS    (72.0f)
+#define PIXPERMETER_ACROSS_BARRIER    (10.0f)
 #define RESAMPLEDIST          (0.02f)
 #define ANGLEDIST             (0.2f)
 #define ANGLEDIST_barrier     (0.06f)

app/gl_cross.c

@@ -5,134 +5,189 @@ enum cross_type_e cross_type = CROSS_NONE;
 bool far_Lpt0_found, far_Lpt1_found;
 int32_t Both_Boder_None_Cross;
 int far_Lpt0_rpts0s_id, far_Lpt1_rpts1s_id;
+int cross_easy = 0;
+float cross_open = 1;
 
-void CheckCross() {
+void CheckCross()
+{
     bool Xfound = Lpt0_found && Lpt1_found;
-    if (cross_type == CROSS_NONE && Xfound){ 
+    if ((int)cross_open == 0) {
-    cross_type = CROSS_BEGIN;
+        if (cross_type == CROSS_NONE && Xfound) {
+            cross_type = CROSS_BEGIN;
+        }
+    }
+    if ((int)cross_open == 1) {
+        if (cross_type == CROSS_NONE && Xfound && cross_easy == 0) {
+            cross_type = CROSS_BEGIN;
+        }
+        if (cross_type == CROSS_NONE && cross_easy == 1 && (((Lpt0_found || Lpt1_found) && (pts_resample_left_count <= 3 || pts_resample_right_count <= 3)) || Xfound)) {
+            cross_type = CROSS_BEGIN;
+        }
     }
 }
 
-void RunCross() {
+void RunCross()
+{
     bool Xfound = Lpt0_found && Lpt1_found;
-    //检测到十字，先按照近线走
+    // 检测到十字，先按照近线走
-    if (cross_type == CROSS_BEGIN) {
+    if (cross_easy == 0) {
-        if (Lpt0_found) {
+        if (cross_type == CROSS_BEGIN) {
-            mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
+            if (Lpt0_found) {
-            track_type = TRACK_LEFT;
+                mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
-        }
+                track_type                               = TRACK_LEFT;
-        else if(Lpt1_found) {
+            } else if (Lpt1_found) {
-            mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id;
+                mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id;
-            track_type = TRACK_RIGHT;
+                track_type                                 = TRACK_RIGHT;
-        }
-
-        //近角点过少，进入远线控制
-        if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count <20 && pts_resample_right_count<20)) {
-            cross_type = CROSS_IN;
-        }
-        
-    }
-        //远线控制进十字,begin_y渐变靠近防丢线
-    if (cross_type == CROSS_IN) {
-        //寻远线,算法与近线相同
-        cross_farline();
-
-        if (pts_resample_left_count < 5 && pts_resample_right_count < 5) { 
-            Both_Boder_None_Cross++;
             }
-        if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) {
-            cross_type = CROSS_NONE;
-            Both_Boder_None_Cross = 0;
-        }
-        if (far_Lpt1_found) { track_type = TRACK_RIGHT; }
-        else if (far_Lpt0_found) { track_type = TRACK_LEFT; }
-        else if (Both_Boder_None_Cross > 0 && pts_far_resample_right_count < 5) { track_type = TRACK_RIGHT; }
-        else if (Both_Boder_None_Cross > 0 && pts_far_resample_left_count < 5) { track_type = TRACK_LEFT; }
 
+            // 近角点过少，进入远线控制
+            if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count < 20 && pts_resample_right_count < 20)) {
+                cross_type = CROSS_IN;
+            }
+        }
+        // 远线控制进十字,begin_y渐变靠近防丢线
+        if (cross_type == CROSS_IN) {
+            // 寻远线,算法与近线相同
+            cross_farline();
+
+            if (pts_resample_left_count < 5 && pts_resample_right_count < 5) {
+                Both_Boder_None_Cross++;
+            }
+            if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) {
+                cross_type            = CROSS_NONE;
+                Both_Boder_None_Cross = 0;
+                cross_easy            = 1;
+            }
+            if (far_Lpt1_found) {
+                track_type = TRACK_RIGHT;
+            } else if (far_Lpt0_found) {
+                track_type = TRACK_LEFT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) {
+                track_type = TRACK_RIGHT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) {
+                track_type = TRACK_LEFT;
+            }
+        }
+    }
+
+    if (cross_easy == 1) {
+        if (cross_type == CROSS_BEGIN) {
+            if (Lpt0_found) {
+                mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
+                track_type                               = TRACK_LEFT;
+            } else if (Lpt1_found) {
+                mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id;
+                track_type                                 = TRACK_RIGHT;
+            }
+
+            // 近角点过少，进入远线控制
+            if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count < 20 && pts_resample_right_count < 20)) {
+                cross_type = CROSS_IN;
+            }
+        }
+        // 远线控制进十字,begin_y渐变靠近防丢线
+        if (cross_type == CROSS_IN) {
+            // 寻远线,算法与近线相同
+            cross_farline();
+
+            if (pts_resample_left_count < 5 && pts_resample_right_count < 5) {
+                Both_Boder_None_Cross++;
+            }
+            if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) {
+                cross_type            = CROSS_NONE;
+                Both_Boder_None_Cross = 0;
+                cross_easy            = 0;
+            }
+            if (far_Lpt1_found) {
+                track_type = TRACK_RIGHT;
+            } else if (far_Lpt0_found) {
+                track_type = TRACK_LEFT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) {
+                track_type = TRACK_RIGHT;
+            } else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) {
+                track_type = TRACK_LEFT;
+            }
+        }
     }
 }
 
-
+void cross_farline()
-void cross_farline(){
+{
     int cross_width = 4;
     int far_y1 = 0, far_y2 = 0;
     int far_x1 = 0, far_x2 = 0;
-    bool white_found = false;
+    bool white_found   = false;
     pts_far_left_count = sizeof(pts_far_left) / sizeof(pts_far_left[0]);
 
     int w1 = IMAGE_W / 2 - BEGINW_R, h1 = BEGINH_L;
-    for(;w1>cross_width*2; w1--) 
+    for (; w1 > cross_width * 2; w1--) {
-    {
+        if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) {
-        if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) {
             far_x1 = w1 - cross_width;
             break;
-        }   
+        }
     }
     /*如果一行全为白色没写*/
-    if (far_x1 <= 30)
+    if (far_x1 <= 4) {
-    {
+        far_x1 = 4;
-        far_x1 = 30;
     }
 
     for (; h1 > 0; h1--) {
-        //先黑后白，先找white
+        // 先黑后白，先找white
         if (GET_PIX_1C(mt9v03x_image_copy[0], h1, far_x1) >= FIX_BINTHRESHOLD) { white_found = true; }
         if (GET_PIX_1C(mt9v03x_image_copy[0], h1 - 1, far_x1) < FIX_BINTHRESHOLD && white_found) {
             far_y1 = h1;
             break;
         }
     }
-    if (GET_PIX_1C(mt9v03x_image_copy[0], far_y1, far_x1) >= FIX_BINTHRESHOLD){
+    if (GET_PIX_1C(mt9v03x_image_copy[0], far_y1, far_x1) >= FIX_BINTHRESHOLD) {
         SearchLineAdaptive_Left(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, far_y1, far_x1, pts_far_left, &pts_far_left_count);
-    }
+    } else
-    else pts_far_left_count = 0;
+        pts_far_left_count = 0;
 
     int w2 = IMAGE_W / 2 + BEGINW_L, h2 = BEGINH_R;
-    white_found = false;
+    white_found         = false;
     pts_far_right_count = sizeof(pts_far_right) / sizeof(pts_far_right[0]);
-    for(;w2 < IMAGE_W - cross_width*2; w2++) 
+    for (; w2 < IMAGE_W - cross_width * 2; w2++) {
-    {
+        if (GET_PIX_1C(mt9v03x_image_copy[0], h2, w2 + 1) < FIX_BINTHRESHOLD) {
-        if(GET_PIX_1C(mt9v03x_image_copy[0], h2, w2 + 1) < FIX_BINTHRESHOLD) {
             far_x2 = w2 + cross_width;
             break;
-        }   
+        }
     }
     /*如果一行全为白色没写*/
-    if (far_x2 >= 120)
+    if (far_x2 >= 136) {
-    {
+        far_x2 = 136;
-        far_x2 = 120;
     }
 
     for (; h2 > 0; h2--) {
-        //先黑后白，先找white
+        // 先黑后白，先找white
         if (GET_PIX_1C(mt9v03x_image_copy[0], h2, far_x2) >= FIX_BINTHRESHOLD) { white_found = true; }
         if (GET_PIX_1C(mt9v03x_image_copy[0], h2 - 1, far_x2) < FIX_BINTHRESHOLD && white_found) {
             far_y2 = h2;
             break;
         }
     }
-    if (GET_PIX_1C(mt9v03x_image_copy[0], far_y2, far_x2) >= FIX_BINTHRESHOLD){
+    if (GET_PIX_1C(mt9v03x_image_copy[0], far_y2, far_x2) >= FIX_BINTHRESHOLD) {
         SearchLineAdaptive_Right(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, far_y2, far_x2, pts_far_right, &pts_far_right_count);
-    }
+    } else
-    else pts_far_right_count = 0;
+        pts_far_right_count = 0;
 
-    //透视变换
+    // 透视变换
     for (int i = 0; i < pts_far_left_count; i++) {
-        pts_far_inv_l[i][1] = fclip(InverseMapW[pts_far_left[i][0]][pts_far_left[i][1]],0,IMAGE_W);
+        pts_far_inv_l[i][1] = fclip(InverseMapW[pts_far_left[i][0]][pts_far_left[i][1]], 0, IMAGE_W);
-        pts_far_inv_l[i][0] = fclip(InverseMapH[pts_far_left[i][0]][pts_far_left[i][1]],0,IMAGE_H);
+        pts_far_inv_l[i][0] = fclip(InverseMapH[pts_far_left[i][0]][pts_far_left[i][1]], 0, IMAGE_H);
     }
     pts_far_inv_l_count = pts_far_left_count;
 
     for (int i = 0; i < pts_far_right_count; i++) {
-        pts_far_inv_r[i][1] = fclip(InverseMapW[pts_far_right[i][0]][pts_far_right[i][1]],0,IMAGE_W);
+        pts_far_inv_r[i][1] = fclip(InverseMapW[pts_far_right[i][0]][pts_far_right[i][1]], 0, IMAGE_W);
-        pts_far_inv_r[i][0] = fclip(InverseMapH[pts_far_right[i][0]][pts_far_right[i][1]],0,IMAGE_H);
+        pts_far_inv_r[i][0] = fclip(InverseMapH[pts_far_right[i][0]][pts_far_right[i][1]], 0, IMAGE_H);
     }
     pts_far_inv_r_count = pts_far_right_count;
 
     // 边线滤波
-    GetLinesFilter(pts_far_inv_l, pts_far_inv_l_count, pts_far_filter_l, (int) round(FILTER_KERNELSIZE));
+    GetLinesFilter(pts_far_inv_l, pts_far_inv_l_count, pts_far_filter_l, (int)round(FILTER_KERNELSIZE));
     pts_far_filter_l_count = pts_far_inv_l_count;
-    GetLinesFilter(pts_far_inv_r, pts_far_inv_r_count, pts_far_filter_r, (int) round(FILTER_KERNELSIZE));
+    GetLinesFilter(pts_far_inv_r, pts_far_inv_r_count, pts_far_filter_r, (int)round(FILTER_KERNELSIZE));
     pts_far_filter_r_count = pts_far_inv_r_count;
 
     // 边线等距采样
@@ -141,14 +196,13 @@ void cross_farline(){
     pts_far_resample_right_count = sizeof(pts_far_resample_right) / sizeof(pts_far_resample_right[0]);
     GetLinesResample(pts_far_filter_r, pts_far_filter_r_count, pts_far_resample_right, &pts_far_resample_right_count, RESAMPLEDIST * PIXPERMETER);
 
-
     int valid_far_points_count = 0;
 
     for (int i = 0; i < pts_far_resample_left_count; i++) {
         int x = pts_far_resample_left[i][1];
         int y = pts_far_resample_left[i][0];
 
-        if (y < IMAGE_H-3 && x > 3 && x < IMAGE_W-3 && y>=3 ) {
+        if (y < IMAGE_H - 3 && x > 3 && x < IMAGE_W - 3 && y >= 3) {
             pts_far_resample_left[valid_far_points_count][0] = pts_far_resample_left[i][0];
             pts_far_resample_left[valid_far_points_count][1] = pts_far_resample_left[i][1];
             valid_far_points_count++;
@@ -162,7 +216,7 @@ void cross_farline(){
         int x = pts_far_resample_right[i][1];
         int y = pts_far_resample_right[i][0];
 
-        if (y <= IMAGE_H-3 && x >= 3 && x <= IMAGE_W-3 && y>=3) {
+        if (y <= IMAGE_H - 3 && x >= 3 && x <= IMAGE_W - 3 && y >= 3) {
             pts_far_resample_right[valid_far_points_count][0] = pts_far_resample_right[i][0];
             pts_far_resample_right[valid_far_points_count][1] = pts_far_resample_right[i][1];
             valid_far_points_count++;
@@ -172,47 +226,43 @@ void cross_farline(){
     pts_far_resample_right_count = valid_far_points_count;
 
     // 边线角度变化率
-    local_angle_points(pts_far_resample_left, pts_far_resample_left_count, far_angle_left, (int) round(ANGLEDIST / RESAMPLEDIST));
+    local_angle_points(pts_far_resample_left, pts_far_resample_left_count, far_angle_left, (int)round(ANGLEDIST / RESAMPLEDIST));
     far_angle_left_num = pts_far_resample_left_count;
-    local_angle_points(pts_far_resample_right, pts_far_resample_right_count, far_angle_right, (int) round(ANGLEDIST / RESAMPLEDIST));
+    local_angle_points(pts_far_resample_right, pts_far_resample_right_count, far_angle_right, (int)round(ANGLEDIST / RESAMPLEDIST));
     far_angle_right_num = pts_far_resample_right_count;
 
     // 角度变化率非极大值抑制
-    nms_angle(far_angle_left, far_angle_left_num, far_angle_new_left, (int) round(ANGLEDIST / RESAMPLEDIST) * 2 + 1);
+    nms_angle(far_angle_left, far_angle_left_num, far_angle_new_left, (int)round(ANGLEDIST / RESAMPLEDIST) * 2 + 1);
     far_angle_new_left_num = far_angle_left_num;
-    nms_angle(far_angle_right, far_angle_right_num, far_angle_new_right, (int) round(ANGLEDIST / RESAMPLEDIST) * 2 + 1);
+    nms_angle(far_angle_right, far_angle_right_num, far_angle_new_right, (int)round(ANGLEDIST / RESAMPLEDIST) * 2 + 1);
     far_angle_new_right_num = far_angle_right_num;
-    
+
     // 找远线上的L角点
     far_Lpt0_found = far_Lpt1_found = false;
     // is_far_straight0 = pts_far_resample_left_count > 1.0 / RESAMPLEDIST;
     // is_far_straight1 = pts_far_resample_right_count > 1.0 / RESAMPLEDIST;
     for (int i = 0; i < pts_far_resample_left_count; i++) {
         if (far_angle_new_left[i] == 0) continue;
-        int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1);
+        int im1    = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1);
-        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1);
+        int ip1    = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1);
         float conf = fabs(far_angle_left[i]) - (fabs(far_angle_left[im1]) + fabs(far_angle_left[ip1])) / 2;
 
-        //L角点阈值
+        // L角点阈值
         if (far_Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.7 / RESAMPLEDIST) {
             far_Lpt0_rpts0s_id = i;
-            far_Lpt0_found = true;
+            far_Lpt0_found     = true;
         }
     }
 
-
     for (int i = 0; i < pts_far_resample_right_count; i++) {
         if (far_angle_new_right[i] == 0) continue;
-        int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1);
+        int im1    = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1);
-        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1);
+        int ip1    = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1);
         float conf = fabs(far_angle_right[i]) - (fabs(far_angle_right[im1]) + fabs(far_angle_right[ip1])) / 2;
 
-
         if (far_Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.7 / RESAMPLEDIST) {
             far_Lpt1_rpts1s_id = i;
-            far_Lpt1_found = true;
+            far_Lpt1_found     = true;
         }
-
     }
-
 }

app/gl_cross.h

@@ -10,6 +10,7 @@ enum cross_type_e {
 extern enum cross_type_e cross_type;
 extern int32_t Both_Boder_None_Cross;
 extern int far_Lpt0_rpts0s_id, far_Lpt1_rpts1s_id;
+extern float cross_open;
 
 void CheckCross(void);
 void RunCross(void); 

app/gl_data.c

@@ -47,11 +47,15 @@ int32_t pts_far_resample_right_count;
 
 float mid_left[PT_MAXLEN][2];
 float mid_right[PT_MAXLEN][2];
+float mid_left_barrier[PT_MAXLEN][2];
+float mid_right_barrier[PT_MAXLEN][2];
 
 float mid_s[PT_MAXLEN][2];
 
 int32_t mid_left_count;
 int32_t mid_right_count;
+int32_t mid_left_barrier_count;
+int32_t mid_right_barrier_count;
 
 float angle_new_left[PT_MAXLEN];
 float angle_new_right[PT_MAXLEN];
@@ -121,4 +125,6 @@ float aim_distance;
 float aim_judge_far=0.3f;
 float FIX_BINTHRESHOLD = 140;
 
-track_type_e track_type = TRACK_RIGHT;
+track_type_e track_type = TRACK_RIGHT;
+
+int begin_flag = 0;

app/gl_data.h

@@ -57,9 +57,14 @@ extern int32_t pts_far_resample_right_count;
 extern float mid_left[PT_MAXLEN][2];
 extern float mid_right[PT_MAXLEN][2];
 extern float mid_s[PT_MAXLEN][2];
+extern float mid_left_barrier[PT_MAXLEN][2];
+extern float mid_right_barrier[PT_MAXLEN][2];
 
 extern int32_t mid_left_count;
 extern int32_t mid_right_count;
+extern int32_t mid_left_barrier_count;
+extern int32_t mid_right_barrier_count;
+
 
 extern float angle_new_left[PT_MAXLEN];
 extern float angle_new_right[PT_MAXLEN];
@@ -132,5 +137,6 @@ extern float aim_judge_far;
 
 extern track_type_e track_type;
 extern float FIX_BINTHRESHOLD;
+extern int begin_flag;
 
 #endif

app/gl_garage.c

@@ -44,7 +44,7 @@ void CheckGarage()
     int banmaxian_hangshu = 0;//斑马线行数
 
     //从下往上扫描
-        for (int y = BEGINH_L - 6; y >= BEGINH_L - 9; y--)
+        for (int y = BEGINH_L + 4; y >= BEGINH_L - 4; y--)
         {
             int banmaxian_kuandu=0;
             //int banmaxian_hangshu=0;

app/gl_garage.h

@@ -5,6 +5,7 @@
 enum garage_type_e {
   GARAGE_NONE, // 非车库模式
   GARAGE_FOUND,
+  OUT_STOP,
 };
 extern enum garage_type_e garage_type;
 

app/gl_get_corners.c

@@ -61,7 +61,15 @@ void get_corners()
         // L 角点阈值
         if (Lpt0_found_barrier == false && (40. / 180. * PI32) < conf && conf < (160. / 180. * PI32) && i < 0.6 / RESAMPLEDIST) {
             Lpt0_rpts0s_id_barrier = i;
-            Lpt0_found_barrier     = true;
+            if (pts_resample_left[i][1] <= pts_resample_left[clip(i + 2, 0, pts_resample_left_count - 1)][1])
+            {
+                Lpt0_found_barrier     = true;
+            }
+            else{
+                Lpt0_found_barrier     = false;
+            }
+            
+            //Lpt0_found_barrier     = true;
         }
     }
     if (Lpt0_found_barrier) {
@@ -73,6 +81,9 @@ void get_corners()
         }
     }
 
+
+
+
     for (int i = 0; i < pts_resample_right_count; i++) {
         if (angle_new_right[i] == 0) continue;
         int im1    = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
@@ -118,7 +129,15 @@ void get_corners()
 
         if (Lpt1_found_barrier == false && (40. / 180. * PI32) < conf && conf < 160. / 180. * PI32 && i < 0.6 / RESAMPLEDIST) {
             Lpt1_rpts1s_id_barrier = i;
-            Lpt1_found_barrier     = true;
+
+            if (pts_resample_right[i][1] >= pts_resample_right[clip(i + 2, 0, pts_resample_left_count - 1)][1])
+            {
+                Lpt1_found_barrier     = true;
+            }
+            else{
+                Lpt1_found_barrier     = false;
+            }
+            //Lpt1_found_barrier     = true;
         }
     }
     if (Lpt1_found_barrier) {

app/gl_img_process.c

@@ -146,5 +146,11 @@ void img_processing() {
     GetMidLine_Right(pts_resample_right, pts_resample_right_count, mid_right, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER_ACROSS * ROADWIDTH / 2);
     mid_right_count = pts_resample_right_count;
 
+    // 左右中线跟踪(避障专属)
+    GetMidLine_Left(pts_resample_left, pts_resample_left_count, mid_left_barrier, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER_ACROSS_BARRIER * ROADWIDTH / 2);
+    mid_left_barrier_count = pts_resample_left_count;
+    GetMidLine_Right(pts_resample_right, pts_resample_right_count, mid_right_barrier, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER_ACROSS_BARRIER * ROADWIDTH / 2);
+    mid_right_barrier_count = pts_resample_right_count;
+
 
 }

app/gl_tracking.c

@@ -15,6 +15,9 @@ int rpts_num;
 float last_pure_angle = 0.0f;
 int8_t turn_flag      = 0;
 int circle_flag       = 1;
+int pingbi_num        = 0;
+int last_garage_type  = GARAGE_NONE;
+float turn_threshold = 12.0f;
 // 计算最小二乘法斜率的函数
 float leastSquaresSlope(float points[][2], int n)
 {
@@ -112,23 +115,27 @@ void aim_distance_select(void)
 
 void ElementJudge()
 {
-    CheckGarage();
+
-    if (garage_type == GARAGE_NONE) {
+    if (shield_type == SHIELD_NONE) {
-        CheckCross();
+        CheckGarage();
-        if (cross_type == CROSS_NONE) {
+        if (garage_type == GARAGE_NONE) {
-            CheckBarrier();
+            CheckCross();
-            if (barrier_type == BARRIER_NONE) {
+            if (cross_type == CROSS_NONE) {
-                CheckCircle();
+                CheckBarrier();
-                if (circle_type == CIRCLE_NONE) {
+                if (barrier_type == BARRIER_NONE) {
-                    Check_s();
+                    CheckCircle();
+                    if (circle_type == CIRCLE_NONE) {
+                        Check_s();
+                    }
                 }
             }
         }
     }
 
     if (garage_type != GARAGE_NONE) {
-        cross_type  = CROSS_NONE;
+        cross_type   = CROSS_NONE;
-        circle_type = CIRCLE_NONE;
+        circle_type  = CIRCLE_NONE;
+        barrier_type = BARRIER_NONE;
     }
     if (cross_type != CROSS_NONE) {
         circle_type  = CIRCLE_NONE;
@@ -147,12 +154,10 @@ void ElementRun()
 
     else if (cross_type != CROSS_NONE) {
         RunCross();
-    }
-
-    else if (circle_type != CIRCLE_NONE) {
-        RunCircle();
     } else if (barrier_type != BARRIER_NONE) {
         RunBarrier();
+    } else if (circle_type != CIRCLE_NONE) {
+        RunCircle();
     } else if (s_type != S_NONE) {
         // RunS();
     }
@@ -160,6 +165,11 @@ void ElementRun()
 
 void MidLineTrack()
 {
+    // if (shield_type == SHIELD_BEGIN) {
+    //     shield_type = SHIELD_NONE;
+    //     pure_angle = 0.f;
+    //     system_delay_ms(3500);  // 斑马线发车延时
+    // }
     if (cross_type == CROSS_IN) {
         if (track_type == TRACK_LEFT) {
             mid_track = mid_left; // 这是为了预先分配内存
@@ -170,6 +180,14 @@ void MidLineTrack()
             GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
             mid_track_count = pts_far_resample_right_count - far_Lpt1_rpts1s_id;
         }
+    } else if (barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_RUNNING) {
+        if (track_type == TRACK_LEFT) {
+            mid_track       = mid_left_barrier;
+            mid_track_count = mid_left_barrier_count;
+        } else {
+            mid_track       = mid_right_barrier;
+            mid_track_count = mid_right_barrier_count;
+        }
     } else {
         if (track_type == TRACK_LEFT) {
             mid_track       = mid_left;
@@ -274,17 +292,10 @@ void MidLineTrack()
         switch (barrier_type) {
             case BARRIER_LEFT_BEGIN:
                 // dx_near    = mid_track[aim_idx_near][1] - cx + barrier_offset;
-                pure_angle = -45.f;
+                pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
-                break;
-            case BARRIER_LEFT_RUNNING:
-                pure_angle = 25.f;
                 break;
             case BARRIER_RIGHT_BEGIN:
-                pure_angle = 45.f;
+                pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
-                break;
-            case BARRIER_RIGHT_RUNNING:
-                // dx_near    = mid_track[aim_idx_near][1] - cx - barrier_offset;
-                pure_angle = -25.f;
             default:
                 // pure_angle  = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
                 if (dy > 0) {
@@ -296,28 +307,14 @@ void MidLineTrack()
                     // pure_angle_half = last_pure_angle_half;
                 }
         }
-
+        if (garage_type == GARAGE_FOUND) {
-        // if (barrier_type == BARRIER_LEFT_BEGIN) {
+            pure_angle = 0.f;
-        //     // dx_near    = mid_track[aim_idx_near][1] - cx + barrier_offset;
+        }
-        //     pure_angle = -25.f;
+        if (cross_type == CROSS_IN)
-        // } else if (barrier_type == BARRIER_LEFT_RUNNING) {
+        {
-        //     pure_angle = 25.f;
+            pure_angle      = -atanf(dx / (dy / 2)) / PI32 * 180.0f;
-        // } else if (barrier_type == BARRIER_RIGHT_BEGIN) {
+        }
-        //     pure_angle = 25.f;
+        
-        // } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
-        //     // dx_near    = mid_track[aim_idx_near][1] - cx - barrier_offset;
-        //     pure_angle = -25.f;
-        // } else {
-        //     // pure_angle  = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
-        //     if (dy > 0) {
-        //         pure_angle      = -atanf(dx / dy) / PI32 * 180.0f;
-        //         last_pure_angle = pure_angle;
-        //         // last_pure_angle_half = pure_angle_half;
-        //     } else {
-        //         pure_angle = last_pure_angle;
-        //         // pure_angle_half = last_pure_angle_half;
-        //     }
-        // }
 
         // // 计算近锚点偏差值
         // dx_near = rptsn[aim_idx_near][1] - cx;
@@ -326,12 +323,12 @@ void MidLineTrack()
         // // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
     }
 
-    if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_RIGHT_IN) {
+    if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
         state_type = CIRCLE_STATE;
         timer_clear(TIM_3);
         timer_start(TIM_3);
     }
-    if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) {
+    if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT) {
         if (circle_flag == 1) {
             state_type = CIRCLE_STATE;
         }
@@ -352,6 +349,10 @@ void MidLineTrack()
         circle_flag = 1;
     }
 
+    if (circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) {
+        state_type = STRAIGHT_STATE;
+    }
+
     if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
         state_type = BARRIER_STATE;
     }
@@ -359,26 +360,58 @@ void MidLineTrack()
     if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
         state_type = STRAIGHT_STATE;
     }
-    // last_state = state_type;
+
     if (state_type == STRAIGHT_STATE) {
         aim_distance = straight_aim;
     } else if (state_type == TURN_STATE) {
         aim_distance = turn_aim;
     }
+    if (cross_type == CROSS_IN)
+    {
+        aim_distance = cross_aim;
+    }
+    if (curvature >= turn_threshold)
+    {
+        aim_distance = turn_aim;
+    }
+    if (curvature <= turn_threshold)
+    {
+        aim_distance = mid_aim;
+    }
+    
+    
+    
+    
 
-    // if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) {
+    if (garage_type == GARAGE_FOUND) {
-    //     turn_flag = 1;
+        last_garage_type = GARAGE_FOUND;
-    //     timer_clear(TIM_3);
+    }
-    //     timer_start(TIM_3);
-    // }
-    // if (turn_flag == 1) {
-    //     aim_distance = turn_aim;
 
-    //     uint16 ti = timer_get(TIM_3);
+    if (last_garage_type == GARAGE_FOUND) {
-    //     if (ti >= 2000) {
+        pingbi_num++;
-    //         turn_flag = 0;
+        if (pingbi_num >= 300)
-    //         timer_stop(TIM_3);
+        {
-    //         timer_clear(TIM_3);
+            pingbi_num = 300;
-    //     }
+        }
-    // }
+        
+        if (pingbi_num >= 100) {
+            if (state_type != BARRIER_STATE || garage_type != GARAGE_FOUND) {
+                if ((pts_left_count <= 2) && (pts_right_count <= 2) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 - BEGINW_R) <= FIX_BINTHRESHOLD) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 + BEGINW_L) <= FIX_BINTHRESHOLD)) {
+                    garage_type = OUT_STOP;
+                }
+            } 
+        }
+        else{
+            if (garage_type == OUT_STOP)
+            {
+                garage_type = GARAGE_FOUND;
+            }
+        }
+    } else {
+        if (state_type != BARRIER_STATE || garage_type != GARAGE_FOUND) {
+            if ((pts_left_count <= 2) && (pts_right_count <= 2) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 - BEGINW_R) <= FIX_BINTHRESHOLD) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 + BEGINW_L) <= FIX_BINTHRESHOLD)) {
+                garage_type = OUT_STOP;
+            }
+        } 
+    }
 }

app/gl_tracking.h

@@ -10,6 +10,7 @@ extern float curvature;
 
 extern int8_t turn_flag ;
 extern float curvature;
+extern float turn_threshold;
 void aim_distance_select(void);
 void tracking(void);
 void ElementJudge(void);

app/isr.c

@@ -34,6 +34,7 @@
  ********************************************************************************************************************/
 
 #include "zf_common_headfile.h"
+#include "gl_headfile.h"
 #include "by_button.h"
 #include "by_buzzer.h"
 #include "by_frame.h"
@@ -92,15 +93,11 @@ void USART1_IRQHandler(void)
 void USART2_IRQHandler(void)
 {
     if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
-        // uint8_t data = (uint8_t)USART_ReceiveData(USART2);
+        by_frame_parse_uart_handle((uint8_t)USART_ReceiveData(USART2));
-        // by_frame_parse_uart_handle(data);
-        USART_ReceiveData(USART2);
-        NVIC_SystemReset();
         USART_ClearITPendingBit(USART2, USART_IT_RXNE);
     }
 
     if (USART_GetFlagStatus(USART2, USART_FLAG_ORE) != RESET) {
-
         USART_ClearFlag(USART2, USART_FLAG_ORE); // <20><><EFBFBD><EFBFBD> ORE <20><>־λ
         USART_ReceiveData(USART2);
     }
@@ -289,8 +286,8 @@ void EXTI15_10_IRQHandler(void)
 void TIM1_UP_IRQHandler(void)
 {
     if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) {
-        by_frame_send(&tiny_frame_param[0].u32);
         TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
+        by_frame_send(&tiny_frame_param[0].u32);
         by_led_warn_blink();
     }
 }

app/jj_param.c

@@ -10,6 +10,7 @@ soft_iic_info_struct eeprom_param;
 TYPE_UNION iic_buffer[DATA_IN_FLASH_NUM];
 TYPE_UNION tiny_frame_param[20];
 uint32_t *addre[2];
+float auto_exp;
 /**
  * @brief 参数初始化注册
  *
@@ -28,6 +29,9 @@ void jj_param_eeprom_init(void)
     PARAM_REG(aim_straight, &straight_aim, EFLOAT, 1, "str:");
     PARAM_REG(aim_turn, &turn_aim, EFLOAT, 1, "tur:");
     PARAM_REG(baoguang, &FIX_BINTHRESHOLD, EFLOAT, 1, "bao:");
+    PARAM_REG(auto_exp_param, &auto_exp, EFLOAT, 1, "exp:");
+    PARAM_REG(barr_time, &barrirer_time, EFLOAT, 1, "bar:");
+    PARAM_REG(cross_sw, &cross_open, EFLOAT, 1, "cro_sw:");
     jj_param_read(); // 注冊
 }
 /**

app/jj_param.h

@@ -29,7 +29,9 @@ typedef enum {
     Page2_head,
     // 第二页参数
     baoguang = Page2_head,
-
+    auto_exp_param,
+    barr_time,
+    cross_sw,
     Page3_head,
     DATA_IN_FLASH_NUM,
 
@@ -65,4 +67,5 @@ void jj_param_eeprom_init(void);
 void jj_param_write(void);
 void jj_param_read(void);
 extern float data7;
+extern float auto_exp;
 #endif

app/main.c

@@ -29,14 +29,15 @@
 #include "by_led.h"
 #include "jj_param.h"
 #include "jj_blueteeth.h"
+#include "zf_device_config.h"
 
 int main(void)
 {
-
+    TYPE_UNION recv_data[BY_FRAME_DATA_NUM_RECV];
     clock_init(SYSTEM_CLOCK_144M);
     system_delay_init();
     debug_init();
-    mt9v03x_init();
+
     ips200_init(IPS200_TYPE_SPI);
 
     by_led_init();
@@ -46,14 +47,26 @@ int main(void)
     jj_bt_init();
     jj_param_eeprom_init();
 
+    mt9v03x_init();
+    mt9v03x_sccb_set_exposure_time((uint16_t)auto_exp);
+
     Page_Init();
 
     pit_ms_init(TIM1_PIT, 10);
     timer_init(TIM_3, TIMER_MS);
+    timer_init(TIM_2, TIMER_MS);
     by_frame_init();
 
     printf("start running\r\n");
     while (1) {
+        memset(recv_data, 0, sizeof(recv_data));
+        by_frame_parse(&recv_data[0].u32);
+        if (0x0D == recv_data[0].u32) {
+            NVIC_SystemReset();
+        } else if (0x5C == recv_data[0].u32) {
+            begin_flag = 1;
+        }
+
         Page_Run();
         by_buzzer_run();
         tiny_frame_param[0].f32   = pure_angle;

app/page/page.c

@@ -130,7 +130,7 @@ void Page_Init(void)
     PAGE_REG(page_menu, "main");
     PAGE_REG(page_rtcam, "rtcam");
     PAGE_REG(page_param1, "aim_param");
-    PAGE_REG(page_param2, "param2");
+    PAGE_REG(page_param2, "exp");
     PAGE_REG(page_dparam, "dparam");
     PAGE_REG(page_reset, "reset");
     // PAGE_REG(page_argv);

app/page/page_dparam.c

@@ -53,7 +53,7 @@ static void Loop()
     ips200_show_int(90, 18 + 38, track_type, 1);
     ips200_show_int(90, 18 + 56, garage_type, 1);
     ips200_show_int(90, 18 + 74, cross_type, 1);
-    ips200_show_int(90, 18 + 92, circle_type, 1);
+    ips200_show_int(90, 18 + 92, circle_type, 2);
     ips200_show_int(90, 18 + 110, s_type, 1);
     ips200_show_float(90, 18 + 128, curvature, 4, 5);
     ips200_show_int(90, 18 + 146, Lpt0_found_barrier_in, 1);

app/page/page_rtcam.c

@@ -63,6 +63,7 @@ static void Loop()
     ips200_show_uint(160, 224, Lpt1_found_barrier_in, 3);
     ips200_show_uint(200, 165, barrier_type, 3);
     ips200_show_uint(200, 185, state_type, 5);
+    //ips200_show_uint(200, 205, ceshi, 5);
 }
 
 /**

app/page/page_sparam1.c

@@ -33,7 +33,7 @@ static void Setup()
     } else if (Get_new_page() == page_param2) {
         pafrist = Page2_head;
         paend   = Page3_head;
-        ips200_show_string(0, 0, "Param2");
+        ips200_show_string(0, 0, "exp");
     }
 
     palong = paend - pafrist;
@@ -83,9 +83,9 @@ static void Event(page_event event)
 
         Curser_Last = Curser;
         if (page_event_forward == event) {
-            Curser--; // 光标上移
+            Curser++; // 光标上移
         } else if (page_event_backward == event) {
-            Curser++; // 光标下移
+            Curser--; // 光标下移
         } else if (page_event_press_short == event) {
             event_flag = 1; // 选中参数
             Print_Curser(Curser, Curser_Last, RGB565_RED);
@@ -102,7 +102,7 @@ static void Event(page_event event)
         }
         Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
     } else if (1 == event_flag) {
-        if (page_event_forward == event) {
+        if (page_event_backward  == event) {
             switch (Param_Data[Curser + pafrist - 1].type) {
                 case EFLOAT:
                     *((float *)(Param_Data[Curser + pafrist - 1].p_data)) += powf(10, index_power);
@@ -116,7 +116,7 @@ static void Event(page_event event)
                 default:
                     break;
             }
-        } else if (page_event_backward == event) {
+        } else if ( page_event_forward== event) {
             switch (Param_Data[Curser + pafrist - 1].type) {
                 case EFLOAT:
                     *((float *)(Param_Data[Curser + pafrist - 1].p_data)) -= powf(10.0f, (float)index_power);

app/page/page_sparam2.c

@@ -33,7 +33,7 @@ static void Setup()
     } else if (Get_new_page() == page_param2) {
         pafrist = Page2_head;
         paend   = Page3_head;
-        ips200_show_string(0, 0, "Param2");
+        ips200_show_string(0, 0, "exp");
     }
 
     palong = paend - pafrist;
@@ -91,15 +91,16 @@ static void Event(page_event event)
 
         Curser_Last = Curser;
         if (page_event_forward == event) {
-            Curser--; // 光标上移
+            Curser++; // 光标上移
         } else if (page_event_backward == event) {
-            Curser++; // 光标下移
+            Curser--; // 光标下移
         } else if (page_event_press_short == event) {
             event_flag = 1; // 选中参数
             Print_Curser(Curser, Curser_Last, RGB565_RED);
             return;
         } else if (page_event_press_long == event) {
             jj_param_write();
+            mt9v03x_set_exposure_time(auto_exp);
             Page_Shift(page_menu);
             return;
         }
@@ -110,7 +111,7 @@ static void Event(page_event event)
         }
         Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
     } else if (1 == event_flag) {
-        if (page_event_forward == event) {
+        if (page_event_backward == event) {
             switch (Param_Data[Curser + pafrist - 1].type) {
                 case EFLOAT:
                     *((float *)(Param_Data[Curser + pafrist - 1].p_data)) += powf(10.0f, (float)index_power);
@@ -124,7 +125,7 @@ static void Event(page_event event)
                 default:
                     break;
             }
-        } else if (page_event_backward == event) {
+        } else if ( page_event_forward == event) {
             switch (Param_Data[Curser + pafrist - 1].type) {
                 case EFLOAT:
                     *((float *)(Param_Data[Curser + pafrist - 1].p_data)) -= powf(10.0f, (float)index_power);
@@ -153,7 +154,7 @@ static void Event(page_event event)
         else if (EUINT32 == Param_Data[Curser + pafrist - 1].type)
             ips200_show_uint(60, Curser * 18 + 2, *((uint32 *)(Param_Data[Curser + pafrist - 1].p_data)), 5);
         else if (EFLOAT == Param_Data[Curser + pafrist - 1].type)
-            ips200_show_float(60, Curser * 18 + 2, *((float *)(Param_Data[Curser + pafrist - 1].p_data)), 4, 5);
+            ips200_show_float(60, Curser * 18 + 2, *((float *)(Param_Data[Curser + pafrist - 1].p_data)), 8, 1);
     }
 }