测试bug

app/gl_barrier.c

@@ -36,7 +36,7 @@ void RunBarrier()
     } else if (barrier_type == BARRIER_LEFT_RUNNING) {
         track_type   = TRACK_RIGHT;
         time_barrier = timer_get(TIM_3);
-        if (time_barrier >= 250) {
+        if (time_barrier >= 220) {
             barrier_type = BARRIER_NONE;
             track_type   = TRACK_RIGHT;
             timer_start(TIM_3);
@@ -59,7 +59,7 @@ void RunBarrier()
     } else if (barrier_type == BARRIER_RIGHT_RUNNING) {
         track_type   = TRACK_LEFT;
         time_barrier = timer_get(TIM_3);
-        if (time_barrier >= 250) {
+        if (time_barrier >= 220) {
             timer_start(TIM_3);
             timer_clear(TIM_3);
             barrier_type = BARRIER_NONE;

app/gl_state.c

@@ -10,4 +10,4 @@ float straight_aim=0.35f;
 float turn_aim=0.35f;  
 float mid_aim=0.5f;
 float barrier_aim=0.5f;
-float barrier_offset=17.0f;
+float barrier_offset=10.0f;

app/gl_state.h

@@ -6,6 +6,8 @@ enum state_type_e {
     TURN_STATE,
     STRAIGHT_STATE,
     CIRCLE_STATE,
+    CIRCLE_RUNNING_STATE,
+    BARRIER_STATE,
 };
 extern enum state_type_e last_state;
 extern enum state_type_e state_type;

app/gl_tracking.c

@@ -177,19 +177,19 @@ void MidLineTrack()
             mid_track       = mid_right;
             mid_track_count = mid_right_count;
         }
-        float x[mid_track_count / 2];
+        float x[mid_track_count];
-        float y[mid_track_count / 2];
+        float y[mid_track_count];
 
-        for (int i = 0; i < mid_track_count / 2; i++) {
+        for (int i = 0; i < mid_track_count; i++) {
             x[i] = mid_track[i][1];
             y[i] = mid_track[i][0];
         }
 
-        if (mid_track_count <= 4) {
+        if (mid_track_count <= 5) {
             curvature = last_curvature;
             /* code */
         } else {
-            curvature = calculate_curvature(x, y, mid_track_count / 2);
+            curvature = calculate_curvature(x, y, mid_track_count);
         }
         last_curvature = curvature;
     }
@@ -223,18 +223,18 @@ void MidLineTrack()
         GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER);
 
         if (cross_type == CROSS_IN) {
-            float x[rptsn_num / 2];
+            float x[rptsn_num];
-            float y[rptsn_num / 2];
+            float y[rptsn_num];
 
-            for (int i = 0; i < rptsn_num / 2; i++) {
+            for (int i = 0; i < rptsn_num; i++) {
                 x[i] = rptsn[i][1];
                 y[i] = rptsn[i][0];
             }
-            if (rptsn_num <= 4) {
+            if (rptsn_num <= 5) {
                 curvature = last_curvature;
                 /* code */
             } else {
-                curvature = calculate_curvature(x, y, rptsn_num / 2);
+                curvature = calculate_curvature(x, y, rptsn_num);
             }
             last_curvature = curvature;
         }
@@ -271,10 +271,10 @@ void MidLineTrack()
         float temp_near = 0;
         if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) {
             dx_near    = mid_track[aim_idx_near][1] - cx + barrier_offset;
-            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 20;
+            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f - 25;
         } else if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) {
             dx_near    = mid_track[aim_idx_near][1] - cx - barrier_offset;
-            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 20;
+            pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn) / PI32 * 180.0f + 25;
         } else {
             // pure_angle  = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f;
             if (dy > 0) {
@@ -293,9 +293,19 @@ void MidLineTrack()
         // // float dn_near = Q_sqrt(dx_near * dx_near + dy_near * dy_near);
         // // float error_near = -atan2f(dx_near, dy_near) * 180 / PI32;
     }
-    if (circle_type == CIRCLE_LEFT_IN || circle_type == CIRCLE_LEFT_OUT || circle_type == CIRCLE_RIGHT_IN || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_LEFT_BEGIN || circle_type == CIRCLE_RIGHT_BEGIN) {
+    if (circle_type == CIRCLE_LEFT_IN ||  circle_type == CIRCLE_RIGHT_IN ) {
         state_type = CIRCLE_STATE;
     }
+    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)
+    {
+        state_type = CIRCLE_RUNNING_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;
+    }
+    
+    
     if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) {
         state_type = STRAIGHT_STATE;
     }

app/main.c

@@ -68,10 +68,10 @@ int main(void)
             state_type = COMMON_STATE;
             img_processing();
             get_corners();
-            aim_distance_select();
             tracking();
             ElementJudge();
             ElementRun();
+            aim_distance_select();
             MidLineTrack();
             //bt_printf("type= %d", (int)curvature);
         }

app/page/page_dparam.c

@@ -56,6 +56,8 @@ static void Loop()
     ips200_show_int(90, 18 + 92, circle_type, 1);
     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);
+    ips200_show_int(90, 18 + 164, Lpt1_found_barrier_in, 1);
 }
 /**
  * @brief  页面事件