From 2b5ea8434e51a55f93db662783e937b01a12ff75 Mon Sep 17 00:00:00 2001 From: Glz <354338225@qq.com> Date: Wed, 26 Jun 2024 17:27:38 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B5=8B=E8=AF=95bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/gl_barrier.c | 4 ++-- app/gl_state.c | 2 +- app/gl_state.h | 2 ++ app/gl_tracking.c | 36 +++++++++++++++++++++++------------- app/main.c | 2 +- app/page/page_dparam.c | 2 ++ 6 files changed, 31 insertions(+), 17 deletions(-) diff --git a/app/gl_barrier.c b/app/gl_barrier.c index b01036e..3abf1dc 100644 --- a/app/gl_barrier.c +++ b/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; diff --git a/app/gl_state.c b/app/gl_state.c index 37f5f04..d195866 100644 --- a/app/gl_state.c +++ b/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; \ No newline at end of file +float barrier_offset=10.0f; \ No newline at end of file diff --git a/app/gl_state.h b/app/gl_state.h index 5c3f28a..66c4c68 100644 --- a/app/gl_state.h +++ b/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; diff --git a/app/gl_tracking.c b/app/gl_tracking.c index 6adcd06..a2bbee0 100644 --- a/app/gl_tracking.c +++ b/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 y[mid_track_count / 2]; + float x[mid_track_count]; + 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 y[rptsn_num / 2]; + float x[rptsn_num]; + 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; } diff --git a/app/main.c b/app/main.c index b6e056f..ff5d22a 100644 --- a/app/main.c +++ b/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); } diff --git a/app/page/page_dparam.c b/app/page/page_dparam.c index 6971125..c25e655 100644 --- a/app/page/page_dparam.c +++ b/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 页面事件