Merge branch 'master' of http://git.isthmus.tk:441/btl143/firmware_violet_zf

app/gl_circle.c

@@ -0,0 +1,161 @@
+#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+enum circle_type_e circle_type = CIRCLE_NONE;
+
+
+int32_t Left_Border_None_Circle = 0;
+int32_t Right_Border_None_Circle = 0;
+
+int32_t Left_Border_Have_Circle = 0;
+int32_t Right_Border_Have_Circle = 0;
+
+int32_t Left_Border_ToLeft_Circle = 0;
+int32_t Right_Border_ToLeft_Circle = 0;
+
+int32_t Left_Border_ToRight_Circle = 0;
+int32_t Right_Border_ToRight_Circle = 0;
+
+
+void CheckCircle() {
+    // 非圆环模式下，单边L角点, 单边长直道
+    if (circle_type == CIRCLE_NONE && Lpt0_found && !Lpt1_found && is_straight1) {
+        circle_type = CIRCLE_LEFT_BEGIN;
+    }
+    if (circle_type == CIRCLE_NONE && !Lpt0_found && Lpt1_found && is_straight0) {
+        circle_type = CIRCLE_RIGHT_BEGIN;
+    }
+}
+
+
+void RunCircle() {
+  if (circle_type == CIRCLE_LEFT_BEGIN) // 左环开始，寻外直道右线
+  {
+    track_type = TRACK_RIGHT;
+
+    // 先丢左线后有线
+    if (pts_resample_left_count < 0.1 / RESAMPLEDIST) {
+      Left_Border_None_Circle++;
+    }
+    if (pts_resample_left_count > 0.5 / RESAMPLEDIST &&Left_Border_None_Circle > FRAMENONE) {
+      Left_Border_Have_Circle++;
+      if (Left_Border_Have_Circle > FRAMENONE) {
+        circle_type = CIRCLE_LEFT_IN;
+        Left_Border_None_Circle = 0;
+        Left_Border_Have_Circle = 0;
+      }
+    }
+  } else if (circle_type == CIRCLE_LEFT_IN) // 入环，寻内圆左线
+  {
+    track_type = TRACK_LEFT;
+
+    if (pts_resample_right[(int32_t)(0.2 / RESAMPLEDIST)][1] -
+            pts_resample_right[0][1] <
+        -2) {
+      Right_Border_ToLeft_Circle++;
+    }
+    if (Right_Border_ToLeft_Circle > FRAMETOLEFT) {
+      circle_type = CIRCLE_LEFT_RUNNING;
+      Right_Border_ToLeft_Circle = 0;
+    }
+  } else if (circle_type == CIRCLE_LEFT_RUNNING) // 正常巡线，寻外圆右线
+  {
+    // Track_Type = TRACK_RIGHT;
+    track_type = TRACK_LEFT; // 看看加一个如果丢线才切换
+    if (Lpt1_found) {
+      pts_resample_right_count = mid_right_count = Lpt1_rpts1s_id;
+    }
+    if (Lpt1_found && Lpt1_rpts1s_id < 0.4 / RESAMPLEDIST) {
+      circle_type = CIRCLE_LEFT_OUT;
+    }
+  } else if (circle_type == CIRCLE_LEFT_OUT) // 出环，寻内圆
+  {
+    track_type = TRACK_LEFT;
+
+    if (is_straight1) // 右线为长直道
+    {
+      circle_type = CIRCLE_LEFT_END;
+    }
+  } else if (circle_type == CIRCLE_LEFT_END) // 走过圆环，寻右线
+  {
+    track_type = TRACK_RIGHT;
+
+    if (pts_resample_left_count < 0.2 / RESAMPLEDIST) // 左线先丢后有
+    {
+      Left_Border_None_Circle++;
+    }
+    if (pts_resample_left_count > 0.6 / RESAMPLEDIST &&
+        Left_Border_None_Circle > FRAMENONE) {
+      circle_type = CIRCLE_NONE;
+      Left_Border_None_Circle = 0;
+      Left_Border_Have_Circle = 0;
+      Right_Border_ToLeft_Circle = 0;
+      Right_Border_ToRight_Circle = 0;
+    }
+  } else if (circle_type == CIRCLE_RIGHT_BEGIN) // 右环控制，前期寻左直道
+  {
+    track_type = TRACK_LEFT;
+
+    // 先丢右线后有线
+    if (pts_resample_right_count < 0.3 / RESAMPLEDIST) {
+      Right_Border_None_Circle++;
+    }
+    if (pts_resample_right_count > 0.5 / RESAMPLEDIST &&
+        Right_Border_None_Circle > FRAMENONE) {
+      Right_Border_Have_Circle++;
+      if (Right_Border_Have_Circle > FRAMENONE) {
+        circle_type = CIRCLE_RIGHT_IN;
+        Right_Border_None_Circle = 0;
+        Right_Border_Have_Circle = 0;
+      }
+    }
+  } else if (circle_type == CIRCLE_RIGHT_IN) // 入右环，寻右内圆环
+  {
+    track_type = TRACK_RIGHT;
+
+    if (pts_resample_left[(int32_t)(0.2 / RESAMPLEDIST)][1] -
+            pts_resample_left[0][1] >
+        2) {
+      Left_Border_ToRight_Circle++;
+    }
+    if (Left_Border_ToRight_Circle > FRAMETORIGHT) {
+      circle_type = CIRCLE_RIGHT_RUNNING;
+      Left_Border_ToRight_Circle = 0;
+    }
+  } else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线
+  {
+    // track_type = TRACK_LEFT;
+    track_type = TRACK_RIGHT; // 看看加一个如果丢线才切换
+    if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点)
+    {
+      pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id;
+    }
+    if (Lpt0_found && Lpt0_rpts0s_id < 0.4 / RESAMPLEDIST) {
+      circle_type = CIRCLE_RIGHT_OUT;
+    }
+  } else if (circle_type == CIRCLE_RIGHT_OUT) // 出环，寻内圆
+  {
+    track_type = TRACK_RIGHT;
+
+    if (is_straight0) // 加个有线长度判断
+    {
+      circle_type = CIRCLE_RIGHT_END;
+    }
+  } else if (circle_type == CIRCLE_RIGHT_END) // 走过圆环，寻左线
+  {
+    track_type = TRACK_LEFT;
+
+    if (pts_resample_right_count < 0.2 / RESAMPLEDIST) // 左线先丢后有
+    {
+      Right_Border_None_Circle++;
+    }
+    if (pts_resample_right_count > 0.7 / RESAMPLEDIST &&
+        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

@@ -0,0 +1,24 @@
+#ifndef CIRCLE
+#define CIRCLE
+
+
+enum circle_type_e {
+  CIRCLE_NONE, // 非圆环模式
+  CIRCLE_LEFT_BEGIN,
+  CIRCLE_RIGHT_BEGIN, // 圆环开始，识别到单侧L角点另一侧长直道。
+  CIRCLE_LEFT_IN,
+  CIRCLE_RIGHT_IN, // 圆环进入，即走到一侧直道，一侧圆环的位置。
+  CIRCLE_LEFT_RUNNING,
+  CIRCLE_RIGHT_RUNNING, // 圆环内部。
+  CIRCLE_LEFT_OUT,
+  CIRCLE_RIGHT_OUT, // 准备出圆环，即识别到出环处的L角点。
+  CIRCLE_LEFT_END,
+  CIRCLE_RIGHT_END // 圆环结束，即再次走到单侧直道的位置。
+};
+
+extern enum circle_type_e circle_type;
+
+void CheckCircle(void);
+void RunCircle(void);
+
+#endif /* COMMON_H_ */

app/gl_common.c

@@ -0,0 +1,15 @@
+#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+
+sint32 limit(sint32 x, sint32 low, sint32 up)
+{
+    return x > up ? up : x < low ? low : x;
+}
+ int clip(int x, int low, int up) {
+     return x > up ? up : x < low ? low : x;
+ }
+
+float fclip(float x, float low, float up) {
+     return x > up ? up : x < low ? low : x;
+ }

app/gl_common.h

@@ -0,0 +1,35 @@
+#ifndef COMMON_H_
+#define COMMON_H_
+
+
+
+#define IMAGE_H                     (MT9V03X_H)
+#define IMAGE_W                     (MT9V03X_W)
+#define BEGINH_L                    (61)
+#define BEGINH_R                    (61)
+#define BEGINW_L                    (-18)
+#define BEGINW_R                    (-12)
+#define PT_MAXLEN                   (75)
+#define GET_PIX_1C(IMG, H, W)       (IMG[(H) * MT9V03X_W + (W)])   //获取像素点的值
+#define FIX_BINTHRESHOLD            (140)                          //设置最开始的阈值
+#define SELFADAPT_KERNELSIZE        (7)                            //巡线区域核大小
+#define FILTER_KERNELSIZE           (7)                            //滤波核大小
+#define SELFADAPT_OFFSET            (8)                           //适应性块大小
+#define PIXPERMETER                 (56)
+#define RESAMPLEDIST                (0.02)
+#define ANGLEDIST                   (0.2)
+#define ROADWIDTH                   (0.45)
+#define FRAMENONE                   (1)
+#define FRAMETOLEFT                 (5)
+#define FRAMETORIGHT                (5)
+
+typedef signed int          sint32;
+typedef float               float32;
+
+sint32 limit(sint32 x, sint32 low, sint32 up);
+int clip(int x, int low, int up);
+float fclip(float x, float low, float up) ;
+
+
+
+#endif /* COMMON_H_ */

app/gl_cross.c

@@ -0,0 +1,57 @@
+#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+enum cross_type_e cross_type = CROSS_NONE;
+bool far_Lpt0_found, far_Lpt1_found;
+int32_t Both_Boder_None_Cross;
+
+void CheckCross() {
+    bool Xfound = Lpt0_found && Lpt1_found;
+    if (cross_type == CROSS_NONE && Xfound){ 
+    cross_type = CROSS_BEGIN;
+    }
+}
+
+void RunCross() {
+    bool Xfound = Lpt0_found && Lpt1_found;
+    aim_distance = 0.4;
+    //检测到十字，先按照近线走
+    if (cross_type == CROSS_BEGIN) {
+        if (Lpt0_found) {
+            mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id;
+            track_type = TRACK_LEFT;
+        }
+        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;
+            //cross_encoder = current_encoder;
+        }
+        
+    }
+        //远线控制进十字,begin_y渐变靠近防丢线
+    else 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_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(){}

app/gl_cross.h

@@ -0,0 +1,18 @@
+#ifndef CROSS
+#define CROSS
+
+
+enum cross_type_e {
+  CROSS_NONE,  // 非十字模式
+  CROSS_BEGIN, // 找到左右两个L角点
+  CROSS_IN, // 两个L角点很近，即进入十字内部(此时切换远线控制)
+};
+extern enum cross_type_e cross_type;
+extern int32_t Both_Boder_None_Cross;
+
+void CheckCross(void);
+void RunCross(void); 
+void cross_farline(void);
+
+
+#endif /* COMMON_H_ */

app/gl_garage.c

@@ -0,0 +1,23 @@
+#include "zf_common_headfile.h"
+#include "math.h"
+#include "gl_headfile.h"
+
+enum garage_type_e garage_type = GARAGE_NONE;
+
+float (*garage_rpts)[2];
+int garage_rpts_num;
+
+float calculate_vector_angle(float x1, float y1, float x2, float y2) {
+    float dx = x2 - x1;
+    float dy = y2 - y1;
+
+    float vector_length = sqrt(dx*dx + dy*dy);
+    float angle_radians = acos(dx / vector_length);
+    float angle_degrees = angle_radians * 180 / M_PI;
+
+    return angle_degrees;
+}
+
+void CheckGarage() {}
+
+void RunGarage(){}

app/gl_garage.h

@@ -0,0 +1,23 @@
+#ifndef GARAGE
+#define GARAGE
+
+
+enum garage_type_e {
+  GARAGE_NONE, // 非车库模式
+  GARAGE_OUT_LEFT,
+  GARAGE_OUT_RIGHT, // 出库，陀螺仪转过45°，即出库完毕
+  GARAGE_FOUND_LEFT,
+  GARAGE_FOUND_RIGHT, // 发现车库，即斑马线+单侧L角点(未使用)
+  GARAGE_IN_LEFT,
+  GARAGE_IN_RIGHT, // 进库，发现车库后判断第几次，从而决定是否进库
+  GARAGE_PASS_LEFT,
+  GARAGE_PASS_RIGHT, // 不进库，发现车库后判断第几次，从而决定是否进库
+  GARAGE_STOP // 进库完毕，停车
+};
+extern enum garage_type_e garage_type;
+
+float calculate_vector_angle(float x1, float y1, float x2, float y2);\
+void CheckGarage(void);
+void RunGarage(void);
+
+#endif /* COMMON_H_ */

app/gl_get_corners.c

@@ -0,0 +1,88 @@
+#include "zf_common_headfile.h"
+#include "math.h"
+#include "gl_headfile.h"
+
+
+
+void get_corners() {
+    //
+    Lpt0_found = Lpt1_found = false;
+    Lpt_in0_found = Lpt_in1_found = false;
+    is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST;
+    is_straight1 = pts_resample_right_count > 1.0 / RESAMPLEDIST;
+    for (int i = 0; i < pts_resample_left_count; i++) {
+        if (angle_new_left[i] == 0) continue;
+        int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
+        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1);
+        float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2;
+
+        //L角点阈值
+        if (Lpt0_found == false && (66. / 180. * PI) < conf && conf < (140. / 180. * PI) && i < 0.7 / RESAMPLEDIST) {
+            Lpt0_rpts0s_id = i;
+            Lpt0_found = true;
+            transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]);
+        }
+        //长直道阈值
+        if (conf > (7. / 180. * PI) && i < 0.8 / RESAMPLEDIST) is_straight0 = false;
+        if (Lpt0_found == true && is_straight0 == false) break;
+    }
+    if(Lpt0_found){
+        float angle1 = calculate_vector_angle(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],pts_resample_left[Lpt0_rpts0s_id+5][1],pts_resample_left[Lpt0_rpts0s_id+5][0]);
+        if(angle1 < 85.)    {
+            Lpt_in0_found = true;
+            Lpt0_found = false;
+            Lpt_in0_rpts0s_id = Lpt0_rpts0s_id;
+        }
+    }
+    else{
+        is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05);
+        if(is_turn0){
+            state_type = TURN_STATE;
+        }
+    }
+    if(is_straight0){
+        state_type = STRAIGHT_STATE;
+    }
+
+
+
+
+    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);
+        int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1);
+        float conf = fabs(angle_right[i]) - (fabs(angle_right[im1]) + fabs(angle_right[ip1])) / 2;
+
+
+        if (Lpt1_found == false && (66. / 180. * PI) < conf && conf < 140. / 180. * PI && i < 0.7 / RESAMPLEDIST) {
+            Lpt1_rpts1s_id = i;
+            Lpt1_found = true;
+            transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]);
+        }
+
+        if (conf > (7. / 180. * PI) && i < 0.8 / RESAMPLEDIST) is_straight1 = false;
+        if (Lpt1_found == true && is_straight1 == false) break;
+    }
+    if(Lpt1_found){
+        float angle2 = calculate_vector_angle(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],pts_resample_right[Lpt1_rpts1s_id+5][1],pts_resample_right[Lpt1_rpts1s_id+5][0]);
+        if(angle2 > 100.)   {
+            Lpt_in1_found = true;
+            Lpt1_found = false;
+            Lpt_in1_rpts1s_id = Lpt1_rpts1s_id;
+        }
+    }else{
+        is_turn1 = is_curve(angle_right, clip(angle_right_num - 10,0 ,angle_right_num), 0.05);
+        if(is_turn1){
+            state_type = TURN_STATE;
+        }
+    }
+    if(is_straight1){
+        state_type = STRAIGHT_STATE;
+    }
+
+    //
+
+
+}
+
+

app/gl_get_corners.h

@@ -0,0 +1,8 @@
+#ifndef GET_CORNERS
+#define GET_CORNERS
+
+
+void get_corners(void);
+
+
+#endif /* COMMON_H_ */

app/gl_handle_img.c

@@ -0,0 +1,236 @@
+#include "zf_common_headfile.h"
+#include "math.h"
+#include "gl_headfile.h"
+
+
+
+
+const sint32 direction_front[4][2] = {{0,  -1},{1,  0},{0,  1},{-1, 0}};
+const sint32 direction_frontleft[4][2] = {{-1, -1},{1,  -1},{1,  1},{-1, 1}};
+const sint32 direction_frontright[4][2] ={{1,  -1},{1,  1},{-1, 1},{-1, -1}};
+
+
+void SearchLineAdaptive_Left(uint8 img_gray[], sint32 block_size, sint32 down_value, sint32 h, sint32 w, sint32 pts[][2], sint32* line_num)
+{
+    int half = block_size / 2;
+    int step = 0, dir = 0, turn = 0;
+    while (step < *line_num && half < w && w < IMAGE_W - half -1 && half < h && h < IMAGE_H - half -1 && turn < 4) {
+        int local_thres = 0;
+        for (int dh = -half; dh <= half; dh++) {
+            for (int dw = -half; dw <= half; dw++) {
+                local_thres += GET_PIX_1C(img_gray, h + dh, w + dw);
+            }
+        }
+        local_thres /= block_size * block_size;
+        local_thres -= down_value;
+
+        int front_value = GET_PIX_1C(img_gray, h+direction_front[dir][1], w + direction_front[dir][0]);
+        int frontleft_value = GET_PIX_1C(img_gray, h + direction_frontleft[dir][1], w + direction_frontleft[dir][0]);
+        if (front_value < local_thres) {
+            dir = (dir + 1) % 4;
+            turn++;
+        } else if (frontleft_value < local_thres) {
+            w += direction_front[dir][0];
+            h += direction_front[dir][1];
+            pts[step][1] = w;
+            pts[step][0] = h;
+            step++;
+            turn = 0;
+        } else { 
+
+            w += direction_frontleft[dir][0];
+            h += direction_frontleft[dir][1];
+            dir = (dir + 3) % 4;
+            pts[step][1] = w;
+            pts[step][0] = h;
+            step++;
+            turn = 0;
+			
+        }
+
+
+    }
+    *line_num = step;
+}
+
+void SearchLineAdaptive_Right(uint8 img_gray[], sint32 block_size, sint32 down_value, sint32 h, sint32 w, sint32 pts[][2], sint32* line_num)
+{
+    int half = block_size / 2;
+        int step = 0, dir = 0, turn = 0;
+        while (step < *line_num && 0 < w && w < IMAGE_W - 1   && 0 < h && h < IMAGE_H -1  && turn < 4) {
+            int local_thres = 0;
+            for (int dh = -half; dh <= half; dh++) {
+                for (int dw = -half; dw <= half; dw++) {
+                    local_thres += GET_PIX_1C(img_gray, h + dh, w + dw);
+                }
+            }
+            local_thres /= block_size * block_size;
+            local_thres -= down_value;
+
+
+            int front_value = GET_PIX_1C(img_gray, h + direction_front[dir][1], w + direction_front[dir][0]);
+            int frontright_value = GET_PIX_1C(img_gray, h + direction_frontright[dir][1], w + direction_frontright[dir][0]);
+            if (front_value < local_thres) {
+                dir = (dir + 3) % 4;
+                turn++;
+            } else if (frontright_value < local_thres) {
+                w += direction_front[dir][0];
+                h += direction_front[dir][1];
+                pts[step][1] = w;
+                pts[step][0] = h;
+                step++;
+                turn = 0;
+
+
+            } else {
+                w += direction_frontright[dir][0];
+                h += direction_frontright[dir][1];
+                dir = (dir + 1) % 4;
+                pts[step][1] = w;
+                pts[step][0] = h;
+                step++;
+                turn = 0;
+
+
+            }
+
+        }
+        *line_num = step;
+}
+
+
+
+void GetLinesFilter(float32 pts_in[][2], sint32 pts_in_count, float32 pts_out[][2], sint32 kernelsize)
+{
+        int half = kernelsize / 2;
+        for (int i = 0; i < pts_in_count; i++) {
+            pts_out[i][0] = 0;
+            pts_out[i][1] = 0;
+            for (int j = -half; j <= half; j++) {
+                pts_out[i][0] += pts_in[clip(i + j, 0, pts_in_count - 1)][0] * (half + 1 - abs(j));
+                pts_out[i][1] += pts_in[clip(i + j, 0, pts_in_count - 1)][1] * (half + 1 - abs(j));
+            }
+            pts_out[i][0] /= (2 * half + 2) * (half + 1) / 2;
+            pts_out[i][1] /= (2 * half + 2) * (half + 1) / 2;
+        }
+}
+
+void GetLinesResample(float32 pts_in[][2], sint32 num1, float32 pts_out[][2], sint32* num2, float32 dist)
+{
+    if (num1 < 0) {
+            *num2 = 0;
+            return;
+        }
+        pts_out[0][0] = pts_in[0][0];
+        pts_out[0][1] = pts_in[0][1];
+        int len = 1;
+        for (int i = 0; i < num1 - 1 && len < *num2; i++) {
+            float x0 = pts_in[i][1];
+            float y0 = pts_in[i][0];
+            float x1 = pts_in[i + 1][1];
+            float y1 = pts_in[i + 1][0];
+
+            do {
+                float x = pts_out[len - 1][1];
+                float y = pts_out[len - 1][0];
+
+                float dx0 = x0 - x;
+                float dy0 = y0 - y;
+                float dx1 = x1 - x;
+                float dy1 = y1 - y;
+
+                float dist0 = sqrt(dx0 * dx0 + dy0 * dy0);
+                float dist1 = sqrt(dx1 * dx1 + dy1 * dy1);
+
+                float r0 = (dist1 - dist) / (dist1 - dist0);
+                float r1 = 1 - r0;
+
+                if (r0 < 0 || r1 < 0) break;
+                x0 = x0 * r0 + x1 * r1;
+                y0 = y0 * r0 + y1 * r1;
+                pts_out[len][1] = x0;
+                pts_out[len][0] = y0;
+                len++;
+            } while (len < *num2);
+
+        }
+        *num2 = len;
+}
+
+
+void local_angle_points(float pts_in[][2], int num, float angle_out[], int dist) {
+    for (int i = 0; i < num; i++) {
+        if (i <= 0 || i >= num - 1) {
+            angle_out[i] = 0;
+            continue;
+        }
+        float dx1 = pts_in[i][0] - pts_in[clip(i - dist, 0, num - 1)][0];
+        float dy1 = pts_in[i][1] - pts_in[clip(i - dist, 0, num - 1)][1];
+        float dn1 = sqrtf(dx1 * dx1 + dy1 * dy1);
+        float dx2 = pts_in[clip(i + dist, 0, num - 1)][0] - pts_in[i][0];
+        float dy2 = pts_in[clip(i + dist, 0, num - 1)][1] - pts_in[i][1];
+        float dn2 = sqrtf(dx2 * dx2 + dy2 * dy2);
+        float c1 = dx1 / dn1;
+        float s1 = dy1 / dn1;
+        float c2 = dx2 / dn2;
+        float s2 = dy2 / dn2;
+        angle_out[i] = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);   //得到弧度 类似angle_out = 0.3491
+    }
+}
+
+void nms_angle(float angle_in[], int num, float angle_out[], int kernel) {
+
+    int half = kernel / 2;
+    for (int i = 0; i < num; i++) {
+        angle_out[i] = angle_in[i];
+        for (int j = -half; j <= half; j++) {
+            if (fabs(angle_in[clip(i + j, 0, num - 1)]) > fabs(angle_out[i])) {
+                angle_out[i] = 0;
+                break;
+            }
+        }
+    }
+}
+
+void GetMidLine_Left(float32 pts_left[][2], sint32 pts_left_count, float32 mid_left[][2], sint32 approx_num, float32 dist)
+{
+    for (int i = 0; i < pts_left_count; i++) {
+        float dx = pts_left[clip(i + approx_num, 0, pts_left_count - 1)][1] - pts_left[clip(i - approx_num, 0, pts_left_count - 1)][1];
+        float dy = pts_left[clip(i + approx_num, 0, pts_left_count - 1)][0] - pts_left[clip(i - approx_num, 0, pts_left_count - 1)][0];
+        float dn = sqrt(dx * dx + dy * dy);
+        dx /= dn;
+        dy /= dn;
+        mid_left[i][0] = pts_left[i][0] + dx * dist;
+        mid_left[i][1] = pts_left[i][1] - dy * dist;
+    }
+
+}
+
+
+void GetMidLine_Right(float32 pts_right[][2], sint32 pts_right_count, float32 mid_right[][2], sint32 approx_num, float32 dist)
+{
+    for (int i = 0; i < pts_right_count; i++) {
+        float dx = pts_right[clip(i + approx_num, 0, pts_right_count -1)][1] - pts_right[clip(i - approx_num, 0, pts_right_count -1 )][1];
+        float dy = pts_right[clip(i + approx_num, 0, pts_right_count -1)][0] - pts_right[clip(i - approx_num, 0, pts_right_count -1)][0];
+        float dn = sqrt(dx * dx + dy * dy);
+        dx /= dn;//sin
+        dy /= dn;//cos
+            mid_right[i][0] = pts_right[i][0] - dx * dist;
+            mid_right[i][1] = pts_right[i][1] + dy * dist;
+
+
+    }
+}
+
+
+int is_curve(float32 angle[], int n, float32 threshold) {
+    for (int i = 1; i < n - 1; i++) {
+        float32 da = fabs(angle[i] - angle[i-1]);
+        float32 db = fabs(angle[i+1] - angle[i]);
+        if (da > threshold && db > threshold) {
+            return 1; // 是弯道
+        }
+    }
+    return 0; // 不是弯道
+}
+

app/gl_handle_img.h

@@ -0,0 +1,15 @@
+#ifndef HANDLE_IMG
+#define HANDLE_IMG
+
+
+
+void SearchStartPoint_Left(uint8 img_gray[], sint32* h_start, sint32* w_start, uint8 threshold);
+void GetLinesFilter(float32 pts_in[][2], sint32 pts_in_count, float32 pts_out[][2], sint32 kernelsize);
+void GetLinesResample(float32 pts_in[][2], sint32 num1, float32 pts_out[][2], sint32* num2, float32 dist);
+void local_angle_points(float pts_in[][2], int num, float angle_out[], int dist);
+void nms_angle(float angle_in[], int num, float angle_out[], int kernel);
+void GetMidLine_Left(float32 pts_left[][2], sint32 pts_left_count, float32 mid_left[][2], sint32 approx_num, float32 dist);
+void GetMidLine_Right(float32 pts_right[][2], sint32 pts_right_count, float32 mid_right[][2], sint32 approx_num, float32 dist);
+int is_curve(float32 angle[], int n, float32 threshold) ;
+
+#endif /* COMMON_H_ */

app/gl_headfile.h

@@ -0,0 +1,21 @@
+#ifndef GL_HEADFILE
+#define GL_HEADFILE
+
+
+
+#include "gl_state.h"
+#include "gl_img_process.h"
+#include "gl_common.h"
+#include "main.h"
+#include "gl_handle_img.h"
+#include "gl_transform_table.h"
+#include "gl_get_corners.h"
+#include "gl_garage.h"
+#include "gl_tracking.h"
+#include "gl_circle.h"
+#include "gl_cross.h"
+#include "math.h"
+
+
+
+#endif /* STATE_H_ */

app/gl_img_process.c

@@ -0,0 +1,113 @@
+#include "zf_common_headfile.h"
+#include "math.h"
+#include "gl_headfile.h"
+/*******************************边线处理****************************************/
+/*
+    @author: glz
+    @brief: 
+
+
+
+*/
+
+
+
+void img_processing() {
+
+    int w1 = IMAGE_W / 2 - BEGINW_R, h1 = BEGINH_L;
+    pts_left_count = sizeof(pts_left) / sizeof(pts_left[0]);
+    for (; w1 > 0; w1--) {
+        if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) 
+            break;
+    }
+
+    if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1) >= FIX_BINTHRESHOLD){
+        SearchLineAdaptive_Left(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h1, w1, pts_left, &pts_left_count);
+    }
+    else pts_left_count = 0;
+
+    int w2 = IMAGE_W / 2 + BEGINW_L, h2 = BEGINH_R;
+    pts_right_count = sizeof(pts_right) / sizeof(pts_right[0]);
+    for (; w2 < IMAGE_W - 1; w2++) {
+        if (GET_PIX_1C(mt9v03x_image_copy[0], h2, w2 + 1) < FIX_BINTHRESHOLD) 
+            break;
+    }
+
+    if (GET_PIX_1C(mt9v03x_image_copy[0], h2, w2) >= FIX_BINTHRESHOLD){
+        SearchLineAdaptive_Right(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, h2, w2, pts_right, &pts_right_count);
+    }
+    else pts_right_count = 0;
+
+    //透视变换
+    for (int i = 0; i < pts_left_count; i++) {
+        pts_inv_l[i][1] = fclip(UndistInverseMapW[pts_left[i][0]][pts_left[i][1]],0,IMAGE_W);
+        pts_inv_l[i][0] = fclip(UndistInverseMapH[pts_left[i][0]][pts_left[i][1]],0,IMAGE_H);
+    }
+    pts_inv_l_count = pts_left_count;
+
+    for (int i = 0; i < pts_right_count; i++) {
+        pts_inv_r[i][1] = fclip(UndistInverseMapW[pts_right[i][0]][pts_right[i][1]],0,IMAGE_W);
+        pts_inv_r[i][0] = fclip(UndistInverseMapH[pts_right[i][0]][pts_right[i][1]],0,IMAGE_H);
+    }
+    pts_inv_r_count = pts_right_count;\
+
+    // 边线滤波
+    GetLinesFilter(pts_inv_l, pts_inv_l_count, pts_filter_l, (int) round(FILTER_KERNELSIZE));
+    pts_filter_l_count = pts_inv_l_count;
+    GetLinesFilter(pts_inv_r, pts_inv_r_count, pts_filter_r, (int) round(FILTER_KERNELSIZE));
+    pts_filter_r_count = pts_inv_r_count;
+
+    // 边线等距采样
+    pts_resample_left_count = sizeof(pts_resample_left) / sizeof(pts_resample_left[0]);
+    GetLinesResample(pts_filter_l, pts_filter_l_count, pts_resample_left, &pts_resample_left_count, RESAMPLEDIST * PIXPERMETER);
+    pts_resample_right_count = sizeof(pts_resample_right) / sizeof(pts_resample_right[0]);
+    GetLinesResample(pts_filter_r, pts_filter_r_count, pts_resample_right, &pts_resample_right_count, RESAMPLEDIST * PIXPERMETER);
+
+    int valid_points_count = 0;
+
+    for (int i = 0; i < pts_resample_left_count; i++) {
+        int x = pts_resample_left[i][1];
+        int y = pts_resample_left[i][0];
+
+        if (y < IMAGE_H-3 && x > 3 && x < IMAGE_W-3 && y>=3 ) {
+            pts_resample_left[valid_points_count][0] = pts_resample_left[i][0];
+            pts_resample_left[valid_points_count][1] = pts_resample_left[i][1];
+            valid_points_count++;
+        }
+    }
+    pts_resample_left_count = valid_points_count;
+    valid_points_count = 0;
+
+    for (int i = 0; i < pts_resample_right_count; i++) {
+        int x = pts_resample_right[i][1];
+        int y = pts_resample_right[i][0];
+
+        if (y <= IMAGE_H-3 && x >= 3 && x <= IMAGE_W-3 && y>=3) {
+            pts_resample_right[valid_points_count][0] = pts_resample_right[i][0];
+            pts_resample_right[valid_points_count][1] = pts_resample_right[i][1];
+            valid_points_count++;
+        }
+    }
+
+    pts_resample_right_count = valid_points_count;
+
+    // 边线角度变化率
+    local_angle_points(pts_resample_left, pts_resample_left_count, angle_left, (int) round(ANGLEDIST / RESAMPLEDIST));
+    angle_left_num = pts_resample_left_count;
+    local_angle_points(pts_resample_right, pts_resample_right_count, angle_right, (int) round(ANGLEDIST / RESAMPLEDIST));
+    angle_right_num = pts_resample_right_count;
+
+    // 角度变化率非极大值抑制
+    nms_angle(angle_left, angle_left_num, angle_new_left, (int) round(ANGLEDIST / RESAMPLEDIST) * 2 + 1);
+    angle_new_left_num = angle_left_num;
+    nms_angle(angle_right, angle_right_num, angle_new_right, (int) round(ANGLEDIST / RESAMPLEDIST) * 2 + 1);
+    angle_new_right_num = angle_right_num;
+
+    // 左右中线跟踪
+    GetMidLine_Left(pts_resample_left, pts_resample_left_count, mid_left, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
+    mid_left_count = pts_resample_left_count;
+    GetMidLine_Right(pts_resample_right, pts_resample_right_count, mid_right, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2);
+    mid_right_count = pts_resample_right_count;
+
+
+}

app/gl_img_process.h

@@ -0,0 +1,12 @@
+#ifndef IMG_PROCESS
+#define IMG_PROCESS
+
+#include "zf_common_headfile.h"
+
+typedef signed int          sint32;
+
+void img_processing(void);
+void SearchLineAdaptive_Left(uint8 img_gray[], sint32 block_size, sint32 down_value, sint32 h, sint32 w, sint32 pts[][2], sint32* line_num);
+void SearchLineAdaptive_Right(uint8 img_gray[], sint32 block_size, sint32 down_value, sint32 h, sint32 w, sint32 pts[][2], sint32* line_num);
+
+#endif /* STATE_H_ */

app/gl_state.c

@@ -0,0 +1,4 @@
+#include "gl_state.h"
+#include "gl_headfile.h"
+
+enum state_type_e state_type =0;

app/gl_state.h

@@ -0,0 +1,22 @@
+#ifndef STATE_H_
+#define STATE_H_
+
+enum state_type_e {
+  COMMON_STATE = 0,
+  CROSS_STATE,HALF_STATE,
+  CIRCLE_IN_STATE,CIRCLE_BEGIN_STATE,
+  CIRCLE_RUNNING_STATE,CIRCLE_OUT_STATE,
+  TURN_STATE,STRAIGHT_STATE,
+  BREAK_STATE,BAR_STATE,
+  RAMP_STATE,GARAGE_OUT_STATE,
+  GARAGE_IN_STATE,GARAGE_STOP_STATE,
+  GARAGE_PASS_STATE,
+};
+
+extern enum state_type_e state_type;
+
+#define CROSS_AIM               (0.49)
+#define COMMON_AIM              (0.31)
+
+
+#endif /* STATE_H_ */

app/gl_tracking.c

@@ -0,0 +1,120 @@
+#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+float (*mid_track)[2];
+int32_t mid_track_count;
+float near_angle;
+float pure_angle;
+float (*rpts)[2];
+int rpts_num;
+
+
+
+void tracking(){
+
+    if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) {
+            track_type = TRACK_RIGHT;
+        } else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) {
+            track_type = TRACK_LEFT;
+        } else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) {
+            track_type = TRACK_RIGHT;
+        } else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) {
+            track_type = TRACK_LEFT;
+        }
+
+}
+
+
+
+void ElementJudge() {
+  CheckGarage();
+  if (garage_type == GARAGE_NONE) {
+    CheckCross();
+    if (cross_type == CROSS_NONE) {
+      CheckCircle();
+    }
+  }
+}
+
+void ElementRun() {
+  if (garage_type != GARAGE_NONE) {
+    RunGarage();
+  }
+
+  else if (cross_type != CROSS_NONE) {
+    RunCross();
+  }
+
+  else if (circle_type != CIRCLE_NONE) {
+    RunCircle();
+  }
+}
+
+void MidLineTrack() {
+  if (cross_type == CROSS_IN) {
+    if (track_type == TRACK_LEFT) {
+    }
+  } 
+  else {
+    if (track_type == TRACK_LEFT) {
+      mid_track = mid_left;
+      mid_track_count = mid_left_count;
+    } else {
+      mid_track = mid_right;
+      mid_track_count = mid_right_count;
+    }
+  }
+
+  // 车轮对应点(纯跟踪起始点)
+  float cx = UndistInverseMapW[(int) (IMAGE_H * 0.90f)][78];
+  float cy = UndistInverseMapH[(int) (IMAGE_H * 0.90f)][78];
+
+  // 找最近点(起始点中线归一化)
+  float min_dist = 1e10;
+
+  int begin_id = -1;
+  for (int i = 0; i < rpts_num; i++) {
+      float dx = rpts[i][1] - cx;
+      float dy = rpts[i][0] - cy;
+      float dist = sqrt(dx * dx + dy * dy);
+      if (dist < min_dist) {
+          min_dist = dist;
+          begin_id = i;
+      }
+  }
+
+  if (begin_id >= 0 && rpts_num - begin_id >= 3){
+    // 归一化中线
+    rpts[begin_id][0] = cy;
+    rpts[begin_id][1] = cx;
+    rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
+    GetLinesResample(rpts + begin_id, rpts_num - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER );
+
+    // 远预锚点位置
+    int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
+
+    //近锚点位置
+    int aim_idx_near = clip(round(0.25 / RESAMPLEDIST), 0, rptsn_num - 1);
+
+    // 计算远锚点偏差值
+    float dx = rptsn[aim_idx][1] - cx;
+    float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER;
+    float dn = sqrt(dx * dx + dy * dy);
+    //float error = -atan2f(dx, dy) * 180 / PI;
+
+    // 计算近锚点偏差值
+    float dx_near = rptsn[aim_idx_near][1] - cx;
+    float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
+    float dn_near = sqrt(dx_near * dx_near + dy_near *    dy_near);
+    //float error_near = -atan2f(dx_near, dy_near) * 180 / PI;
+
+    //考虑近点
+    near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI * 180 ;
+    //考虑远点
+    pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ;  
+
+    
+
+  }
+}
+

app/gl_tracking.h

@@ -0,0 +1,11 @@
+#ifndef TRACKING
+#define TRACKING
+
+
+void tracking(void);
+void ElementJudge(void);
+void ElementRun(void);
+void MidLineTrack(void); 
+
+
+#endif /* STATE_H_ */

app/gl_transform_table.c

@@ -0,0 +1,26 @@
+#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+
+float H_inv[3][3] = {};
+
+// 逆变换，俯视->原图
+void transform(float X, float Y, int* x, int* y) {
+    // Apply inverse transformation
+    double p[3] = {X, Y, 1.0};
+    double q[3] = {0.0, 0.0, 0.0};
+    for (int i = 0; i < 3; i++) {
+        for (int j = 0; j < 3; j++) {
+            q[i] += H_inv[i][j] * p[j];
+        }
+    }
+
+    *x =(int) (q[0] / q[2]);
+    *y = (int) (q[1] / q[2]);
+}
+
+
+const float UndistInverseMapH[90][140]  =   {};
+
+
+const float UndistInverseMapW[90][140]  =   {};

app/gl_transform_table.h

@@ -0,0 +1,14 @@
+#ifndef TRANSFORM_TABLE_H_
+#define TRANSFORM_TABLE_H_
+
+#include "zf_common_headfile.h"
+
+
+extern  const float UndistInverseMapH[90][140] ;
+extern  const float UndistInverseMapW[90][140] ;
+void transform(float X, float Y, int* x, int* y);
+
+
+#endif /* TRANSFORM_TABLE_H_ */
+
+

app/main.c

@@ -33,32 +33,106 @@
  * 2022-09-15        <20><> W            first version
  ********************************************************************************************************************/
 #include "zf_common_headfile.h"
+#include "gl_headfile.h"
 #include "cw_servo.h"
 #include "by_pt_button.h"
 #include "by_fan_control.h"
 
-uint8 mt9v03x_image_cp[MT9V03X_H][MT9V03X_W];
-uint16_t pwm_cnt = 0;
+uint8 (*Img_Gray)[MT9V03X_W]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MT9V03X_W <20>е<EFBFBD> uint8 <20><><EFBFBD>͵Ķ<CDB5>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
+//uint8 *mt9v03x_image_copy[0];               // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> uint8 <20><><EFBFBD>͵<EFBFBD>һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
+sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2];
+sint32 pts_left_count, pts_right_count;
+float32 pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2];
+sint32 pts_inv_l_count, pts_inv_r_count;
+float32 pts_filter_l[PT_MAXLEN][2], pts_filter_r[PT_MAXLEN][2];
+sint32 pts_filter_l_count, pts_filter_r_count;
+float32 pts_resample_left[PT_MAXLEN][2], pts_resample_right[PT_MAXLEN][2];
+sint32 pts_resample_left_count, pts_resample_right_count;
+float32 mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2];
+sint32 mid_left_count, mid_right_count;
+
+
+//<2F><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF>+<2B>Ǽ<EFBFBD><C7BC><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
+float angle_new_left[PT_MAXLEN];
+float angle_new_right[PT_MAXLEN];
+int angle_new_left_num, angle_new_right_num;
+uint8_t mt9v03x_image_copy[MT9V03X_H][MT9V03X_W];
+
+// <20><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF>
+float angle_left[PT_MAXLEN];
+float angle_right[PT_MAXLEN];
+int angle_left_num, angle_right_num;
+
+// L<>ǵ<EFBFBD>
+int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
+bool Lpt0_found, Lpt1_found;
+int Lpt1[2],Lpt0[2];
+
+int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id;
+bool Lpt_in0_found, Lpt_in1_found;
+int Lpt_in1[2], Lpt_in0[2];
+
+// <20><>ֱ<EFBFBD><D6B1>
+bool is_straight0, is_straight1;
+
+// <20><><EFBFBD><EFBFBD>
+bool is_turn0, is_turn1;
+
+// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+float rptsn[PT_MAXLEN][2];
+int rptsn_num;
+float aim_distance;
+
+enum track_type_e track_type = TRACK_RIGHT;
+
+
+int frame_count = 0;
+
+void img_processing();
+void get_corners();
+
 
 int main(void)
 {
     clock_init(SYSTEM_CLOCK_120M); // <20><>ʼ<EFBFBD><CABC>оƬʱ<C6AC><CAB1> <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>Ϊ 120MHz
     debug_init();                  // <20><><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC> MPU ʱ<><CAB1> <20><><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD>
-
+    mt9v03x_init();
     ips114_init();
     by_gpio_init();
     by_exit_init();
     by_pwm_init();
     cw_servo_init();
-    // mt9v03x_init();
-    // <20>˴<EFBFBD><CBB4><EFBFBD>д<EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-
-    
 
     while (1) {
-        // <20>˴<EFBFBD><CBB4><EFBFBD>д<EFBFBD><D0B4>Ҫѭ<D2AA><D1AD>ִ<EFBFBD>еĴ<D0B5><C4B4><EFBFBD>
+        //while (frame_count < 20) {
+            // if (mt9v03x_finish_flag) {
+            //     memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
+            //     adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 8);
+            //     //threshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, MT9V03X_W, MT9V03X_H, 110);
+            //     ips114_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0);
+            //     mt9v03x_finish_flag = 0;
+            //     frame_count++;
+            // }
+
+        //}
+        if (mt9v03x_finish_flag) {
+            //ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0);
+            memcpy(mt9v03x_image_copy[0], mt9v03x_image[0],(sizeof(mt9v03x_image_copy)/sizeof(uint8_t)));
+            //Img_Gray = mt9v03x_image;
+            //mt9v03x_image_copy[0] = Img_Gray[0];
+            mt9v03x_finish_flag = 0;
+
+            state_type = COMMON_STATE;
+            img_processing();
+            get_corners();
+            aim_distance = COMMON_AIM;
+            tracking();
+            ElementJudge();
+            ElementRun();
+            MidLineTrack();
+
+        }
+
         
-        
-        // <20>˴<EFBFBD><CBB4><EFBFBD>д<EFBFBD><D0B4>Ҫѭ<D2AA><D1AD>ִ<EFBFBD>еĴ<D0B5><C4B4><EFBFBD>
     }
-}
+}

app/main.h

@@ -0,0 +1,57 @@
+#ifndef MAIN_H
+#define MAIN_H
+
+#include "zf_common_headfile.h"
+
+
+//extern uint8 *mt9v03x_image_copy[0];
+extern sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2]; //0:H,1:W
+extern sint32 pts_left_count, pts_right_count;
+extern float32 pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2];
+extern sint32 pts_inv_l_count, pts_inv_r_count;
+extern float32 pts_filter_l[PT_MAXLEN][2], pts_filter_r[PT_MAXLEN][2];
+extern sint32 pts_filter_l_count, pts_filter_r_count;
+extern float32 pts_resample_left[PT_MAXLEN][2], pts_resample_right[PT_MAXLEN][2];
+extern sint32 pts_resample_left_count, pts_resample_right_count;
+extern uint8_t mt9v03x_image_copy[MT9V03X_H][MT9V03X_W];
+extern float32 mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2];
+extern sint32 mid_left_count, mid_right_count;
+
+
+extern float angle_left[PT_MAXLEN];
+extern float angle_right[PT_MAXLEN];
+extern int angle_left_num, angle_right_num;
+
+
+extern float angle_new_left[PT_MAXLEN];
+extern float angle_new_right[PT_MAXLEN];
+extern int angle_new_left_num, angle_new_right_num;
+
+
+extern int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
+extern bool Lpt0_found, Lpt1_found;
+extern int Lpt1[2],Lpt0[2];
+
+extern int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id;
+extern bool Lpt_in0_found, Lpt_in1_found;
+extern int Lpt_in1[2], Lpt_in0[2];
+
+extern bool is_straight0, is_straight1;
+
+extern bool is_turn0, is_turn1;
+
+extern float rptsn[PT_MAXLEN][2];
+extern int rptsn_num;
+extern float aim_distance;
+
+
+
+
+enum track_type_e {
+    TRACK_LEFT,
+    TRACK_RIGHT,
+};
+
+extern enum track_type_e track_type;
+
+#endif // MAIN_H