diff --git a/app/gl_barrier.c b/app/gl_barrier.c index 28e7130..fadb26b 100644 --- a/app/gl_barrier.c +++ b/app/gl_barrier.c @@ -3,7 +3,7 @@ enum barrier_type_e barrier_type = BARRIER_NONE; enum s_type_e s_type = S_NONE; -enum shield_type_e shield_type = SHIELD_NONE; +enum shield_type_e shield_type = SHIELD_NONE; float (*mid_track_s)[2]; int32_t mid_track_count_s; int temp_min_s; @@ -13,18 +13,38 @@ 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||barrier_type == BARRIER_LEFT_RUNNING) { + if (barrier_type == BARRIER_LEFT_BEGIN || barrier_type == BARRIER_LEFT_RUNNING) { track_type = TRACK_RIGHT; + if (barrier_type == BARRIER_LEFT_BEGIN) + { + timer_clear(TIM_2); + timer_start(TIM_2); + barrier_type = BARRIER_LEFT_RUNNING; + } + if (barrier_type == BARRIER_LEFT_RUNNING) + { + if (timer_get(TIM_2) >= 500) + { + barrier_type = BARRIER_NONE; + timer_stop(TIM_2); + timer_clear(TIM_2); + } + else{ + barrier_type = BARRIER_LEFT_RUNNING; + } + } + + // if (barrier_begin_flag == 0) { // timer_clear(TIM_3); // timer_start(TIM_3); @@ -46,32 +66,36 @@ void RunBarrier() // } // } // } - if (pts_resample_left_count >= 5) - { - if (Lpt0_found) { - Lpt0_found_count++; - } - 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_2); - timer_start(TIM_2); - } else if (barrier_type == BARRIER_LEFT_RUNNING) { - track_type = TRACK_RIGHT; - if (timer_get(TIM_2) >= 500) { - barrier_type = BARRIER_NONE; - track_type = TRACK_RIGHT; - timer_stop(TIM_2); - timer_clear(TIM_2); - } - } - } - else{ - track_type = TRACK_RIGHT; - barrier_type = BARRIER_NONE; - } - + // if (pts_resample_left_count >= 5) { + // if (Lpt0_found) { + // Lpt0_found_count++; + // } + // 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_2); + // timer_start(TIM_2); + // } else if (barrier_type == BARRIER_LEFT_RUNNING) { + // track_type = TRACK_RIGHT; + // if (timer_get(TIM_2) >= 500) { + // barrier_type = BARRIER_NONE; + // track_type = TRACK_RIGHT; + // timer_stop(TIM_2); + // timer_clear(TIM_2); + // } + // } + // } else { + // track_type = TRACK_RIGHT; + // barrier_type = BARRIER_NONE; + // } + + // if (barrier_type == BARRIER_LEFT_RUNNING) { + // if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) { + // barrier_type = BARRIER_NONE; + // } + // } + // if (Lpt0_found) { // Lpt0_found_count++; // } @@ -92,8 +116,26 @@ void RunBarrier() // } } - if (barrier_type == BARRIER_RIGHT_BEGIN||barrier_type == BARRIER_RIGHT_RUNNING) { + if (barrier_type == BARRIER_RIGHT_BEGIN || barrier_type == BARRIER_RIGHT_RUNNING) { track_type = TRACK_LEFT; + if (barrier_type == BARRIER_RIGHT_BEGIN) + { + timer_clear(TIM_2); + timer_start(TIM_2); + barrier_type = BARRIER_RIGHT_RUNNING; + } + if (barrier_type == BARRIER_RIGHT_RUNNING) + { + if (timer_get(TIM_2) >= 500) + { + barrier_type = BARRIER_NONE; + timer_stop(TIM_2); + timer_clear(TIM_2); + } + else{ + barrier_type = BARRIER_RIGHT_RUNNING; + } + } // if (barrier_begin_flag == 0) { // timer_clear(TIM_3); // timer_start(TIM_3); @@ -115,31 +157,35 @@ void RunBarrier() // } // } // } - if (pts_resample_right_count >= 5) - { - if (Lpt1_found) { - Lpt1_found_count++; - } - 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_2); - timer_start(TIM_2); - } else if (barrier_type == BARRIER_RIGHT_RUNNING) { - track_type = TRACK_LEFT; - if (timer_get(TIM_2) >= 500) { - timer_stop(TIM_2); - timer_clear(TIM_2); - barrier_type = BARRIER_NONE; - track_type = TRACK_LEFT; - } - } - } - else{ - track_type = TRACK_LEFT; - barrier_type = BARRIER_NONE; - } + // if (pts_resample_right_count >= 5) { + // if (Lpt1_found) { + // Lpt1_found_count++; + // } + // 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_2); + // timer_start(TIM_2); + // } else if (barrier_type == BARRIER_RIGHT_RUNNING) { + // track_type = TRACK_LEFT; + // if (timer_get(TIM_2) >= 500) { + // timer_stop(TIM_2); + // timer_clear(TIM_2); + // barrier_type = BARRIER_NONE; + // track_type = TRACK_LEFT; + // } + // } + // } else { + // track_type = TRACK_LEFT; + // barrier_type = BARRIER_NONE; + // } + + // if (barrier_type == BARRIER_RIGHT_RUNNING) { + // if (pts_resample_left_count >= 5 && pts_resample_right_count >= 5) { + // barrier_type = BARRIER_NONE; + // } + // } } } @@ -180,7 +226,6 @@ void RunS() } } - // void Check_shield(){ // if (begin_flag == 1) // { @@ -195,7 +240,5 @@ void RunS() // timer_stop(TIM_3); // timer_clear(TIM_3); // } - - // } \ No newline at end of file diff --git a/app/gl_circle.c b/app/gl_circle.c index b3f1306..bdfff22 100644 --- a/app/gl_circle.c +++ b/app/gl_circle.c @@ -15,6 +15,8 @@ int32_t Right_Border_ToLeft_Circle = 0; int32_t Left_Border_ToRight_Circle = 0; int32_t Right_Border_ToRight_Circle = 0; +int32_t ceshi = 0; + void CheckCircle() { // 非圆环模式下，单边L角点, 单边长直道 @@ -59,7 +61,7 @@ void RunCircle() } } else if (circle_type == CIRCLE_LEFT_RUNNING) // 正常巡线，寻外圆右线 { - //track_type = TRACK_RIGHT; + // track_type = TRACK_RIGHT; track_type = TRACK_LEFT; // 看看加一个如果丢线才切换 if (Lpt1_found) { pts_resample_right_count = mid_right_count = Lpt1_rpts1s_id; @@ -125,8 +127,8 @@ void RunCircle() } else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线 { track_type = TRACK_RIGHT; - //track_type = TRACK_LEFT; // 看看加一个如果丢线才切换 - if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点) + // track_type = TRACK_LEFT; // 看看加一个如果丢线才切换 + if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点) { pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id; } @@ -149,13 +151,13 @@ void RunCircle() { Right_Border_None_Circle++; } - if (pts_resample_right_count > 0.7 / RESAMPLEDIST && - Right_Border_None_Circle > FRAMENONE) { + if (pts_resample_right_count > (0.7 / RESAMPLEDIST) && Right_Border_None_Circle > FRAMENONE) { circle_type = CIRCLE_NONE; Right_Border_None_Circle = 0; Right_Border_Have_Circle = 0; Left_Border_ToLeft_Circle = 0; Left_Border_ToRight_Circle = 0; } + } } diff --git a/app/gl_circle.h b/app/gl_circle.h index 1ddeb7e..b12c266 100644 --- a/app/gl_circle.h +++ b/app/gl_circle.h @@ -17,6 +17,7 @@ enum circle_type_e { }; extern enum circle_type_e circle_type; +extern int32_t ceshi; void CheckCircle(void); void RunCircle(void); diff --git a/app/gl_common.h b/app/gl_common.h index 06966c7..36f1d50 100644 --- a/app/gl_common.h +++ b/app/gl_common.h @@ -16,6 +16,7 @@ #define SELFADAPT_OFFSET (8) // 适应性块大小 #define PIXPERMETER (70.0f) #define PIXPERMETER_ACROSS (72.0f) +#define PIXPERMETER_ACROSS_BARRIER (10.0f) #define RESAMPLEDIST (0.02f) #define ANGLEDIST (0.2f) #define ANGLEDIST_barrier (0.06f) diff --git a/app/gl_cross.c b/app/gl_cross.c index ed53ec2..cef1976 100644 --- a/app/gl_cross.c +++ b/app/gl_cross.c @@ -69,9 +69,9 @@ void cross_farline(){ } } /*如果一行全为白色没写*/ - if (far_x1 <= 30) + if (far_x1 <= 4) { - far_x1 = 30; + far_x1 = 4; } for (; h1 > 0; h1--) { @@ -98,9 +98,9 @@ void cross_farline(){ } } /*如果一行全为白色没写*/ - if (far_x2 >= 120) + if (far_x2 >= 136) { - far_x2 = 120; + far_x2 = 136; } for (; h2 > 0; h2--) { diff --git a/app/gl_data.c b/app/gl_data.c index 0dbf8c3..b9d9d79 100644 --- a/app/gl_data.c +++ b/app/gl_data.c @@ -47,11 +47,15 @@ int32_t pts_far_resample_right_count; float mid_left[PT_MAXLEN][2]; float mid_right[PT_MAXLEN][2]; +float mid_left_barrier[PT_MAXLEN][2]; +float mid_right_barrier[PT_MAXLEN][2]; float mid_s[PT_MAXLEN][2]; int32_t mid_left_count; int32_t mid_right_count; +int32_t mid_left_barrier_count; +int32_t mid_right_barrier_count; float angle_new_left[PT_MAXLEN]; float angle_new_right[PT_MAXLEN]; diff --git a/app/gl_data.h b/app/gl_data.h index 0abc8ee..6706ae6 100644 --- a/app/gl_data.h +++ b/app/gl_data.h @@ -57,9 +57,14 @@ extern int32_t pts_far_resample_right_count; extern float mid_left[PT_MAXLEN][2]; extern float mid_right[PT_MAXLEN][2]; extern float mid_s[PT_MAXLEN][2]; +extern float mid_left_barrier[PT_MAXLEN][2]; +extern float mid_right_barrier[PT_MAXLEN][2]; extern int32_t mid_left_count; extern int32_t mid_right_count; +extern int32_t mid_left_barrier_count; +extern int32_t mid_right_barrier_count; + extern float angle_new_left[PT_MAXLEN]; extern float angle_new_right[PT_MAXLEN]; diff --git a/app/gl_garage.c b/app/gl_garage.c index 68bd14c..d08c779 100644 --- a/app/gl_garage.c +++ b/app/gl_garage.c @@ -44,7 +44,7 @@ void CheckGarage() int banmaxian_hangshu = 0;//斑马线行数 //从下往上扫描 - for (int y = BEGINH_L + 2; y >= BEGINH_L - 2; y--) + for (int y = BEGINH_L + 4; y >= BEGINH_L - 4; y--) { int banmaxian_kuandu=0; //int banmaxian_hangshu=0; diff --git a/app/gl_garage.h b/app/gl_garage.h index 3524a8e..b1a1cbc 100644 --- a/app/gl_garage.h +++ b/app/gl_garage.h @@ -5,6 +5,7 @@ enum garage_type_e { GARAGE_NONE, // 非车库模式 GARAGE_FOUND, + OUT_STOP, }; extern enum garage_type_e garage_type; diff --git a/app/gl_img_process.c b/app/gl_img_process.c index ba65666..0810cfc 100644 --- a/app/gl_img_process.c +++ b/app/gl_img_process.c @@ -146,5 +146,11 @@ void img_processing() { GetMidLine_Right(pts_resample_right, pts_resample_right_count, mid_right, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER_ACROSS * ROADWIDTH / 2); mid_right_count = pts_resample_right_count; + // 左右中线跟踪(避障专属) + GetMidLine_Left(pts_resample_left, pts_resample_left_count, mid_left_barrier, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER_ACROSS_BARRIER * ROADWIDTH / 2); + mid_left_barrier_count = pts_resample_left_count; + GetMidLine_Right(pts_resample_right, pts_resample_right_count, mid_right_barrier, (int) round(ANGLEDIST / RESAMPLEDIST), PIXPERMETER_ACROSS_BARRIER * ROADWIDTH / 2); + mid_right_barrier_count = pts_resample_right_count; + } \ No newline at end of file diff --git a/app/gl_tracking.c b/app/gl_tracking.c index 606d10e..a831325 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -112,7 +112,7 @@ void aim_distance_select(void) void ElementJudge() { - + // if(begin_flag == 1){ // shield_type = SHIELD_BEGIN; // begin_flag = 0; @@ -139,13 +139,16 @@ 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; + } } @@ -183,7 +186,16 @@ 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 { + } 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; + } else { + mid_track = mid_right_barrier; + mid_track_count = mid_right_barrier_count; + } + }else { if (track_type == TRACK_LEFT) { mid_track = mid_left; mid_track_count = mid_left_count; @@ -287,17 +299,10 @@ void MidLineTrack() switch (barrier_type) { case BARRIER_LEFT_BEGIN: // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset; - pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f - 20; - break; - case BARRIER_LEFT_RUNNING: - pure_angle = 15.f; + pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; break; case BARRIER_RIGHT_BEGIN: - 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 = -15.f; + pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; default: // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * 0.5f * dx / dn) / PI32 * 180.0f; if (dy > 0) { @@ -309,11 +314,9 @@ void MidLineTrack() // pure_angle_half = last_pure_angle_half; } } - if (garage_type == GARAGE_FOUND) - { + 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; @@ -349,7 +352,7 @@ void MidLineTrack() timer_clear(TIM_3); timer_start(TIM_3); } - 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) { + if (circle_type == CIRCLE_LEFT_RUNNING || circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_RIGHT_OUT || circle_type == CIRCLE_LEFT_OUT) { if (circle_flag == 1) { state_type = CIRCLE_STATE; } @@ -370,6 +373,12 @@ void MidLineTrack() circle_flag = 1; } + 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; } @@ -399,4 +408,13 @@ void MidLineTrack() // 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; + } + } + else{ + garage_type = GARAGE_NONE; + } } diff --git a/app/page/page_dparam.c b/app/page/page_dparam.c index c25e655..935308a 100644 --- a/app/page/page_dparam.c +++ b/app/page/page_dparam.c @@ -53,7 +53,7 @@ static void Loop() ips200_show_int(90, 18 + 38, track_type, 1); ips200_show_int(90, 18 + 56, garage_type, 1); ips200_show_int(90, 18 + 74, cross_type, 1); - ips200_show_int(90, 18 + 92, circle_type, 1); + ips200_show_int(90, 18 + 92, circle_type, 2); 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); diff --git a/app/page/page_rtcam.c b/app/page/page_rtcam.c index 8d8bf57..6a69c53 100644 --- a/app/page/page_rtcam.c +++ b/app/page/page_rtcam.c @@ -63,6 +63,7 @@ static void Loop() ips200_show_uint(160, 224, Lpt1_found_barrier_in, 3); ips200_show_uint(200, 165, barrier_type, 3); ips200_show_uint(200, 185, state_type, 5); + //ips200_show_uint(200, 205, ceshi, 5); } /**