diff --git a/app/gl_barrier.c b/app/gl_barrier.c index 3abf1dc..d2aada9 100644 --- a/app/gl_barrier.c +++ b/app/gl_barrier.c @@ -8,6 +8,7 @@ float (*mid_track_s)[2]; int32_t mid_track_count_s; int temp_min_s; int temp_max_s; +int barrier_begin_flag = 0; void CheckBarrier() { @@ -23,48 +24,100 @@ void RunBarrier() { if (barrier_type == BARRIER_LEFT_BEGIN) { track_type = TRACK_RIGHT; - if (Lpt0_found) { - Lpt0_found_count++; - } - if (Lpt0_found_count >= 1) { - Lpt0_found_count = 0; - barrier_type = BARRIER_LEFT_RUNNING; - track_type = TRACK_RIGHT; + if (barrier_begin_flag == 0) + { timer_clear(TIM_3); timer_start(TIM_3); + barrier_begin_flag = 1; } - } 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_begin_flag == 1) + { + barrier_type = BARRIER_LEFT_BEGIN; + time_barrier = timer_get(TIM_3); + if (time_barrier >= 1000 && time_barrier < 2000) + { + barrier_type = BARRIER_LEFT_RUNNING; + } + else if (time_barrier >= 2000) + { + barrier_type = BARRIER_NONE; + barrier_begin_flag = 0; + timer_stop(TIM_3); + timer_clear(TIM_3); + } + } + + + // 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 (Lpt1_found) { - Lpt1_found_count++; - } - if (Lpt1_found_count >= 1) { - Lpt1_found_count = 0; - barrier_type = BARRIER_RIGHT_RUNNING; - track_type = TRACK_LEFT; + if (barrier_begin_flag == 0) + { timer_clear(TIM_3); timer_start(TIM_3); + barrier_begin_flag = 1; } - } 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; + + if (barrier_begin_flag == 1) + { + barrier_type = BARRIER_RIGHT_BEGIN; + time_barrier = timer_get(TIM_3); + if (time_barrier >= 1000) + { + barrier_type = BARRIER_RIGHT_RUNNING; + if (time_barrier >= 2000) + { + barrier_type = BARRIER_NONE; + barrier_begin_flag = 0; + timer_stop(TIM_3); + timer_clear(TIM_3); + } + + } + } + + // if (Lpt1_found) { + // Lpt1_found_count++; + // } + // 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; + // } } } diff --git a/app/gl_tracking.c b/app/gl_tracking.c index 05b1b18..9dba0d7 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -270,12 +270,19 @@ void MidLineTrack() float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER; float dn = (dx * dx + dy * dy); 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 - 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 + 25; + if (barrier_type == BARRIER_LEFT_BEGIN) { + //dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset; + pure_angle = -25.f; + }else if (barrier_type == BARRIER_LEFT_RUNNING) + { + pure_angle = 25.f; + }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) {