From 5d56543f6461c3427b832b6f6cd1a29d055fe4d5 Mon Sep 17 00:00:00 2001 From: Glz <354338225@qq.com> Date: Fri, 5 Jul 2024 14:12:01 +0800 Subject: [PATCH] =?UTF-8?q?=E6=97=A5=E5=B8=B8=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/gl_barrier.c | 181 ++++++++++++++++++++++++++-------------------- app/gl_barrier.h | 8 +- app/gl_data.c | 4 +- app/gl_data.h | 1 + app/gl_tracking.c | 42 ++++++----- 5 files changed, 135 insertions(+), 101 deletions(-) diff --git a/app/gl_barrier.c b/app/gl_barrier.c index fa44e92..76f6616 100644 --- a/app/gl_barrier.c +++ b/app/gl_barrier.c @@ -3,110 +3,113 @@ enum barrier_type_e barrier_type = BARRIER_NONE; enum s_type_e s_type = S_NONE; -uint16 time_barrier; +enum shiled_type_e shiled_type = SHILED_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; void CheckBarrier() { - if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) { - barrier_type = BARRIER_LEFT_BEGIN; - } - if (barrier_type == BARRIER_NONE && !Lpt0_found_barrier_in && Lpt1_found_barrier_in && is_straight0) { - barrier_type = BARRIER_RIGHT_BEGIN; - } + if (barrier_type == BARRIER_NONE && Lpt0_found_barrier_in && !Lpt1_found_barrier_in && is_straight1) { + barrier_type = BARRIER_LEFT_BEGIN; + } + if (barrier_type == BARRIER_NONE && !Lpt0_found_barrier_in && Lpt1_found_barrier_in && is_straight0) { + barrier_type = BARRIER_RIGHT_BEGIN; + } } 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_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 (Lpt0_found) { + Lpt0_found_count++; } - } - 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_begin_flag = 0; - timer_stop(TIM_3); - timer_clear(TIM_3); + 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_1); + timer_start(TIM_1); + } else if (barrier_type == BARRIER_LEFT_RUNNING) { + track_type = TRACK_RIGHT; + if (timer_get(TIM_1) >= 220) { + barrier_type = BARRIER_NONE; + track_type = TRACK_RIGHT; + timer_stop(TIM_1); + timer_clear(TIM_1); } } } - // 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_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 (Lpt1_found) { + Lpt1_found_count++; } - } - 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_begin_flag = 0; - timer_stop(TIM_3); - timer_clear(TIM_3); + 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_1); + timer_start(TIM_1); + } else if (barrier_type == BARRIER_RIGHT_RUNNING) { + track_type = TRACK_LEFT; + if (timer_get(TIM_1) >= 220) { + timer_stop(TIM_1); + timer_clear(TIM_1); + barrier_type = BARRIER_NONE; + 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; - // 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 +147,24 @@ void RunS() if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) { s_type = S_NONE; } +} + + +void Check_shiled(){ + if (begin_flag == 1) + { + shiled_type = SHILED_BEGIN; + timer_clear(TIM_3); + timer_start(TIM_3); + begin_flag = 0; + } + if (timer_get(TIM_3) > 1000 && shiled_type == SHILED_BEGIN) + { + shiled_type = SHILED_NONE; + timer_stop(TIM_3); + timer_clear(TIM_3); + } + + + } \ No newline at end of file diff --git a/app/gl_barrier.h b/app/gl_barrier.h index a3fa56f..be96377 100644 --- a/app/gl_barrier.h +++ b/app/gl_barrier.h @@ -18,9 +18,14 @@ enum s_type_e { S_OUT, }; -extern uint16 time_barrier; +enum shiled_type_e { + SHILED_NONE, + SHILED_BEGIN, +}; + extern enum barrier_type_e barrier_type; extern enum s_type_e s_type; +extern enum shiled_type_e shiled_type; extern float (*mid_track_s)[2]; extern int32_t mid_track_count_s; extern int temp_min_s; @@ -29,6 +34,7 @@ extern int temp_max_s; void CheckBarrier(void); void Check_s(void); void RunBarrier(void); +void Check_shiled(void); #endif /* COMMON_H_ */ \ No newline at end of file diff --git a/app/gl_data.c b/app/gl_data.c index 2a4dd9d..0dbf8c3 100644 --- a/app/gl_data.c +++ b/app/gl_data.c @@ -121,4 +121,6 @@ float aim_distance; float aim_judge_far=0.3f; float FIX_BINTHRESHOLD = 140; -track_type_e track_type = TRACK_RIGHT; \ No newline at end of file +track_type_e track_type = TRACK_RIGHT; + +int begin_flag = 0; \ No newline at end of file diff --git a/app/gl_data.h b/app/gl_data.h index d46ae24..0abc8ee 100644 --- a/app/gl_data.h +++ b/app/gl_data.h @@ -132,5 +132,6 @@ extern float aim_judge_far; extern track_type_e track_type; extern float FIX_BINTHRESHOLD; +extern int begin_flag; #endif diff --git a/app/gl_tracking.c b/app/gl_tracking.c index 82ad5b6..1093cc4 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -112,23 +112,27 @@ void aim_distance_select(void) void ElementJudge() { - CheckGarage(); - if (garage_type == GARAGE_NONE) { - CheckCross(); - if (cross_type == CROSS_NONE) { - CheckBarrier(); - if (barrier_type == BARRIER_NONE) { - CheckCircle(); - if (circle_type == CIRCLE_NONE) { - Check_s(); + Check_shiled(); + if (shiled_type == SHILED_NONE) { + CheckGarage(); + if (garage_type == GARAGE_NONE) { + CheckCross(); + if (cross_type == CROSS_NONE) { + CheckBarrier(); + if (barrier_type == BARRIER_NONE) { + CheckCircle(); + if (circle_type == CIRCLE_NONE) { + Check_s(); + } } } } } if (garage_type != GARAGE_NONE) { - cross_type = CROSS_NONE; - circle_type = CIRCLE_NONE; + cross_type = CROSS_NONE; + circle_type = CIRCLE_NONE; + barrier_type = BARRIER_NONE; } if (cross_type != CROSS_NONE) { circle_type = CIRCLE_NONE; @@ -147,12 +151,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(); } @@ -274,17 +276,17 @@ 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 - 20; break; case BARRIER_LEFT_RUNNING: - pure_angle = 25.f; + pure_angle = 15.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 + 20; break; case BARRIER_RIGHT_RUNNING: // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset; - pure_angle = -25.f; + pure_angle = -15.f; default: // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; if (dy > 0) { @@ -326,7 +328,7 @@ 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);