diff --git a/app/gl_cross.c b/app/gl_cross.c index cef1976..fdf4d28 100644 --- a/app/gl_cross.c +++ b/app/gl_cross.c @@ -46,8 +46,8 @@ void RunCross() { } 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_far_resample_right_count < 5) { track_type = TRACK_RIGHT; } - else if (Both_Boder_None_Cross > 0 && pts_far_resample_left_count < 5) { 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; } } } diff --git a/app/gl_tracking.c b/app/gl_tracking.c index a831325..fe6fb9b 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -15,6 +15,8 @@ int rpts_num; float last_pure_angle = 0.0f; int8_t turn_flag = 0; int circle_flag = 1; +int pingbi_num = 0; +int last_garage_type = GARAGE_NONE; // 计算最小二乘法斜率的函数 float leastSquaresSlope(float points[][2], int n) { @@ -113,12 +115,6 @@ void aim_distance_select(void) void ElementJudge() { - // if(begin_flag == 1){ - // shield_type = SHIELD_BEGIN; - // begin_flag = 0; - // return; - // } - if (shield_type == SHIELD_NONE) { CheckGarage(); if (garage_type == GARAGE_NONE) { @@ -139,16 +135,13 @@ void ElementJudge() cross_type = CROSS_NONE; circle_type = CIRCLE_NONE; barrier_type = BARRIER_NONE; - } if (cross_type != CROSS_NONE) { circle_type = CIRCLE_NONE; barrier_type = BARRIER_NONE; - } if (barrier_type != BARRIER_NONE) { circle_type = CIRCLE_NONE; - } } @@ -186,8 +179,7 @@ void MidLineTrack() GetMidLine_Right(pts_far_resample_right + far_Lpt1_rpts1s_id, pts_far_resample_right_count - far_Lpt1_rpts1s_id, mid_right, (int)round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER * ROADWIDTH / 2); mid_track_count = pts_far_resample_right_count - far_Lpt1_rpts1s_id; } - } else if (barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_RUNNING) - { + } else if (barrier_type == BARRIER_LEFT_RUNNING || barrier_type == BARRIER_RIGHT_RUNNING) { if (track_type == TRACK_LEFT) { mid_track = mid_left_barrier; mid_track_count = mid_left_barrier_count; @@ -195,7 +187,7 @@ void MidLineTrack() mid_track = mid_right_barrier; mid_track_count = mid_right_barrier_count; } - }else { + } else { if (track_type == TRACK_LEFT) { mid_track = mid_left; mid_track_count = mid_left_count; @@ -317,28 +309,11 @@ void MidLineTrack() if (garage_type == GARAGE_FOUND) { pure_angle = 0.f; } - - // 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) { - // pure_angle = -atanf(dx / dy) / PI32 * 180.0f; - // last_pure_angle = pure_angle; - // // last_pure_angle_half = pure_angle_half; - // } else { - // pure_angle = last_pure_angle; - // // pure_angle_half = last_pure_angle_half; - // } - // } + if (cross_type == CROSS_IN) + { + pure_angle = -atanf(dx / (dy / 2)) / PI32 * 180.0f; + } + // // 计算近锚点偏差值 // dx_near = rptsn[aim_idx_near][1] - cx; @@ -373,11 +348,9 @@ void MidLineTrack() circle_flag = 1; } - if (circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) - { + if (circle_type == CIRCLE_LEFT_END || circle_type == CIRCLE_RIGHT_END) { state_type = STRAIGHT_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; @@ -386,35 +359,48 @@ void MidLineTrack() if (cross_type == CROSS_BEGIN || cross_type == CROSS_IN) { state_type = STRAIGHT_STATE; } - // last_state = state_type; + if (state_type == STRAIGHT_STATE) { aim_distance = straight_aim; } else if (state_type == TURN_STATE) { aim_distance = turn_aim; } - - // if (last_state == STRAIGHT_STATE && (state_type == TURN_STATE || state_type == CIRCLE_STATE)) { - // turn_flag = 1; - // timer_clear(TIM_3); - // timer_start(TIM_3); - // } - // if (turn_flag == 1) { - // aim_distance = turn_aim; - - // uint16 ti = timer_get(TIM_3); - // if (ti >= 2000) { - // turn_flag = 0; - // timer_stop(TIM_3); - // timer_clear(TIM_3); - // } - // } - - if (state_type != BARRIER_STATE || garage_type != GARAGE_FOUND) { - if ((pts_left_count <= 2) && (pts_right_count <= 2) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 - BEGINW_R) <= FIX_BINTHRESHOLD) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 + BEGINW_L) <= FIX_BINTHRESHOLD)) { - garage_type = OUT_STOP; - } + if (cross_type == CROSS_IN) + { + aim_distance = cross_aim; } - else{ - garage_type = GARAGE_NONE; + + + + if (garage_type == GARAGE_FOUND) { + last_garage_type = GARAGE_FOUND; + } + + if (last_garage_type == GARAGE_FOUND) { + pingbi_num++; + if (pingbi_num >= 300) + { + pingbi_num = 300; + } + + if (pingbi_num >= 100) { + if (state_type != BARRIER_STATE || garage_type != GARAGE_FOUND) { + if ((pts_left_count <= 2) && (pts_right_count <= 2) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 - BEGINW_R) <= FIX_BINTHRESHOLD) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 + BEGINW_L) <= FIX_BINTHRESHOLD)) { + garage_type = OUT_STOP; + } + } + } + else{ + if (garage_type == OUT_STOP) + { + garage_type = GARAGE_FOUND; + } + } + } else { + if (state_type != BARRIER_STATE || garage_type != GARAGE_FOUND) { + if ((pts_left_count <= 2) && (pts_right_count <= 2) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 - BEGINW_R) <= FIX_BINTHRESHOLD) && (GET_PIX_1C(mt9v03x_image_copy[0], BEGINH_L, IMAGE_W / 2 + BEGINW_L) <= FIX_BINTHRESHOLD)) { + garage_type = OUT_STOP; + } + } } }