diff --git a/app/gl_circle.c b/app/gl_circle.c
new file mode 100644
index 0000000..16d69a7
--- /dev/null
+++ b/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;
+ }
+ }
+}
diff --git a/app/gl_circle.h b/app/gl_circle.h
new file mode 100644
index 0000000..1ddeb7e
--- /dev/null
+++ b/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_ */
\ No newline at end of file
diff --git a/app/gl_common.c b/app/gl_common.c
new file mode 100644
index 0000000..29a3138
--- /dev/null
+++ b/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;
+ }
diff --git a/app/gl_common.h b/app/gl_common.h
new file mode 100644
index 0000000..1bd003a
--- /dev/null
+++ b/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_ */
\ No newline at end of file
diff --git a/app/gl_cross.c b/app/gl_cross.c
new file mode 100644
index 0000000..6c08a20
--- /dev/null
+++ b/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(){}
\ No newline at end of file
diff --git a/app/gl_cross.h b/app/gl_cross.h
new file mode 100644
index 0000000..9c5920b
--- /dev/null
+++ b/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_ */
\ No newline at end of file
diff --git a/app/gl_garage.c b/app/gl_garage.c
new file mode 100644
index 0000000..7bbb4a8
--- /dev/null
+++ b/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(){}
\ No newline at end of file
diff --git a/app/gl_garage.h b/app/gl_garage.h
new file mode 100644
index 0000000..ee634c4
--- /dev/null
+++ b/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_ */
\ No newline at end of file
diff --git a/app/gl_get_corners.c b/app/gl_get_corners.c
new file mode 100644
index 0000000..2d8fcd1
--- /dev/null
+++ b/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;
+ }
+
+ //
+
+
+}
+
+
diff --git a/app/gl_get_corners.h b/app/gl_get_corners.h
new file mode 100644
index 0000000..aff31b2
--- /dev/null
+++ b/app/gl_get_corners.h
@@ -0,0 +1,8 @@
+#ifndef GET_CORNERS
+#define GET_CORNERS
+
+
+void get_corners(void);
+
+
+#endif /* COMMON_H_ */
\ No newline at end of file
diff --git a/app/gl_handle_img.c b/app/gl_handle_img.c
new file mode 100644
index 0000000..1e12c62
--- /dev/null
+++ b/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; // 不是弯道
+}
+
diff --git a/app/gl_handle_img.h b/app/gl_handle_img.h
new file mode 100644
index 0000000..05f9470
--- /dev/null
+++ b/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_ */
\ No newline at end of file
diff --git a/app/gl_headfile.h b/app/gl_headfile.h
new file mode 100644
index 0000000..11591ce
--- /dev/null
+++ b/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_ */
\ No newline at end of file
diff --git a/app/gl_img_process.c b/app/gl_img_process.c
new file mode 100644
index 0000000..c0bcbad
--- /dev/null
+++ b/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;
+
+
+}
\ No newline at end of file
diff --git a/app/gl_img_process.h b/app/gl_img_process.h
new file mode 100644
index 0000000..968f2e5
--- /dev/null
+++ b/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_ */
diff --git a/app/gl_state.c b/app/gl_state.c
new file mode 100644
index 0000000..04f94c8
--- /dev/null
+++ b/app/gl_state.c
@@ -0,0 +1,4 @@
+#include "gl_state.h"
+#include "gl_headfile.h"
+
+enum state_type_e state_type =0;
diff --git a/app/gl_state.h b/app/gl_state.h
new file mode 100644
index 0000000..5a3c29f
--- /dev/null
+++ b/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_ */
diff --git a/app/gl_tracking.c b/app/gl_tracking.c
new file mode 100644
index 0000000..b41eb73
--- /dev/null
+++ b/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 ;
+
+
+
+ }
+}
+
diff --git a/app/gl_tracking.h b/app/gl_tracking.h
new file mode 100644
index 0000000..e9f6cb3
--- /dev/null
+++ b/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_ */
diff --git a/app/gl_transform_table.c b/app/gl_transform_table.c
new file mode 100644
index 0000000..7850e2f
--- /dev/null
+++ b/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] = {};
\ No newline at end of file
diff --git a/app/gl_transform_table.h b/app/gl_transform_table.h
new file mode 100644
index 0000000..e269262
--- /dev/null
+++ b/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_ */
+
+
diff --git a/app/main.c b/app/main.c
index d71eb23..923401e 100644
--- a/app/main.c
+++ b/app/main.c
@@ -1,57 +1,170 @@
/*********************************************************************************************************************
-* CH32V307VCT6 Opensourec Library CH32V307VCT6 Դ⣩һڹٷ SDK ӿڵĵԴ
-* Copyright (c) 2022 SEEKFREE ɿƼ
-*
-* ļCH32V307VCT6 Դһ
-*
-* CH32V307VCT6 Դ
-* Ըᷢ GPLGNU General Public License GNUͨù֤
-* GPL ĵ3棨 GPL3.0ѡģκκİ汾·/
-*
-* Դķϣܷãδκεı֤
-* ûԻʺض;ı֤
-* ϸμ GPL
-*
-* ӦյԴͬʱյһ GPL ĸ
-* ûУ
-*
-* ע
-* Դʹ GPL3.0 Դ֤Э Ϊİ汾
-* Ӣİ libraries/doc ļµ GPL3_permission_statement.txt ļ
-* ֤ libraries ļ ļµ LICENSE ļ
-* ӭλʹò ʱ뱣ɿƼİȨ
-*
-* ļ main
-* ˾ ɶɿƼ˾
-* 汾Ϣ 鿴 libraries/doc ļ version ļ 汾˵
-* MounRiver Studio V1.8.1
-* ƽ̨ CH32V307VCT6
-* https://seekfree.taobao.com/
-*
-* ļ¼
-* ע
-* 2022-09-15 W first version
-********************************************************************************************************************/
+ * CH32V307VCT6 Opensourec Library CH32V307VCT6 Դ⣩һڹٷ SDK ӿڵĵԴ
+ * Copyright (c) 2022 SEEKFREE ɿƼ
+ *
+ * ļCH32V307VCT6 Դһ
+ *
+ * CH32V307VCT6 Դ
+ * Ըᷢ GPLGNU General Public License GNUͨù֤
+ * GPL ĵ3棨 GPL3.0ѡģκκİ汾·/
+ *
+ * Դķϣܷãδκεı֤
+ * ûԻʺض;ı֤
+ * ϸμ GPL
+ *
+ * ӦյԴͬʱյһ GPL ĸ
+ * ûУ
+ *
+ * ע
+ * Դʹ GPL3.0 Դ֤Э Ϊİ汾
+ * Ӣİ libraries/doc ļµ GPL3_permission_statement.txt ļ
+ * ֤ libraries ļ ļµ LICENSE ļ
+ * ӭλʹò ʱ뱣ɿƼİȨ
+ *
+ * ļ main
+ * ˾ ɶɿƼ˾
+ * 汾Ϣ 鿴 libraries/doc ļ version ļ 汾˵
+ * MounRiver Studio V1.8.1
+ * ƽ̨ CH32V307VCT6
+ * https://seekfree.taobao.com/
+ *
+ * ļ¼
+ * ע
+ * 2022-09-15 W first version
+ ********************************************************************************************************************/
#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+uint8 (*Img_Gray)[MT9V03X_W]; // ָ MT9V03X_W е uint8 ͵Ķάָ
+//uint8 *mt9v03x_image_copy[0]; // ָ uint8 ͵һάָ
+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;
-int main (void)
+
+//ұ߾ֲǶȱ仯+Ǽֵ
+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];
+
+// ұ߾ֲǶȱ仯
+float angle_left[PT_MAXLEN];
+float angle_right[PT_MAXLEN];
+int angle_left_num, angle_right_num;
+
+// Lǵ
+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];
+
+// ֱ
+bool is_straight0, is_straight1;
+
+//
+bool is_turn0, is_turn1;
+
+// һ
+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();
+void adaptiveThreshold(uint8_t* img_data, uint8_t* output_data, int width, int height, int block, uint8_t clip_value);
+void threshold(uint8_t *img_data, uint8_t *output_data, int width, int height, int thres);
+
+int main(void)
{
- clock_init(SYSTEM_CLOCK_120M); // ʼоƬʱ ƵΪ 120MHz
- debug_init(); // رڳʼMPU ʱ Դ
-
- // ˴дû ʼ
+ clock_init(SYSTEM_CLOCK_120M); // ʼоƬʱ ƵΪ 120MHz
+ debug_init(); // رڳʼMPU ʱ Դ
ips114_init();
mt9v03x_init();
- // ˴дû ʼ
- while(1)
- {
- // ˴дҪѭִеĴ
- if(mt9v03x_finish_flag){
- ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0);
+ while (1) {
+ //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();
+
+
}
- // ˴дҪѭִеĴ
+
+
}
}
+
+/*
+void threshold(uint8_t *img_data, uint8_t *output_data, int width, int height, int thres){
+ for(int y=0; ythres ? 255 : 0;
+ }
+ }
+}
+
+
+void adaptiveThreshold(uint8_t* img_data, uint8_t* output_data, int width, int height, int block, uint8_t clip_value){
+ int half_block = block / 2;
+ for(int y=half_block; ythres ? 255 : 0;
+ }
+ }
+}*/
\ No newline at end of file
diff --git a/app/main.h b/app/main.h
new file mode 100644
index 0000000..ec7ba00
--- /dev/null
+++ b/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
\ No newline at end of file