diff --git a/app/gl_circle.c b/app/gl_circle.c index ea52dd1..ae8cf19 100644 --- a/app/gl_circle.c +++ b/app/gl_circle.c @@ -124,8 +124,8 @@ void RunCircle() } } else if (circle_type == CIRCLE_RIGHT_RUNNING) // 正常巡线，寻外圆左线 { - // track_type = TRACK_RIGHT; - track_type = TRACK_LEFT; // 看看加一个如果丢线才切换 + track_type = TRACK_RIGHT; + //track_type = TRACK_LEFT; // 看看加一个如果丢线才切换 if (Lpt0_found) // 外环存在拐点，可再加拐点距离判据 (左 L 点) { pts_resample_left_count = mid_left_count = Lpt0_rpts0s_id; diff --git a/app/gl_data.c b/app/gl_data.c index 76a1bc9..88680dd 100644 --- a/app/gl_data.c +++ b/app/gl_data.c @@ -103,8 +103,10 @@ bool is_straight1; bool is_far_straight0; bool is_far_straight1; -bool is_turn0; -bool is_turn1; +bool is_turn0_l; +bool is_turn1_l; +bool is_turn0_r; +bool is_turn1_r; float rptsn[PT_MAXLEN][2]; int32_t rptsn_num; diff --git a/app/gl_data.h b/app/gl_data.h index a3aaa8d..04444c5 100644 --- a/app/gl_data.h +++ b/app/gl_data.h @@ -114,8 +114,10 @@ extern int Lpt1_found_barrier_in_id; extern int32_t Lpt0_found_count; extern int32_t Lpt1_found_count; -extern bool is_turn0; -extern bool is_turn1; +extern bool is_turn0_l; +extern bool is_turn1_l; +extern bool is_turn0_r; +extern bool is_turn1_r; extern float rptsn[PT_MAXLEN][2]; extern int32_t rptsn_num; diff --git a/app/gl_garage.c b/app/gl_garage.c index c4e4c3e..16114a6 100644 --- a/app/gl_garage.c +++ b/app/gl_garage.c @@ -40,28 +40,69 @@ float calculate_vector_angle(float x1, float y1, float x2, float y2) void CheckGarage() { - int change_num = 0; - int check_garage_h = 60; - for (int check_garage_w = 50; check_garage_w < IMAGE_W - 50; check_garage_w++) { - if ((GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) >= FIX_BINTHRESHOLD) || - (GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w) >= FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], check_garage_h, check_garage_w + 1) < FIX_BINTHRESHOLD)) { - change_num++; + //变量标志位 + int banmaxian_hangshu = 0;//斑马线行数 + + //从下往上扫描 + for (int y = BEGINH_L - 6; y >= BEGINH_L - 9; y--) + { + int banmaxian_kuandu=0; + //int banmaxian_hangshu=0; + int banmaxian_geshu=0; + //从右往左扫描 + for (int x =130; x >=20; x--) + { + int baidian_heng=0; + //扫描到黑色，就进判断 + if (GET_PIX_1C(mt9v03x_image_copy[0], y, x + 2) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, x + 1) > FIX_BINTHRESHOLD + && GET_PIX_1C(mt9v03x_image_copy[0], y, x) < FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, x - 1) < FIX_BINTHRESHOLD) + { + for (int a = x; a > x - 30; a--)//从黑色点向左侧扫描 + { + //找到白色点 + if(GET_PIX_1C(mt9v03x_image_copy[0], y, a) > FIX_BINTHRESHOLD && GET_PIX_1C(mt9v03x_image_copy[0], y, a - 1) > FIX_BINTHRESHOLD) + { + //记录白色点的位置，跳出循环 + baidian_heng=a; + break; + } + }//斑马线宽度等于黑白点的差 + banmaxian_kuandu=x-baidian_heng; + + + } + else + { //斑马线的宽度在4~8之间认为它成立为斑马线黑色块 + if (banmaxian_kuandu >= 2 && banmaxian_kuandu <= 6) + { + //斑马线黑色块++ + banmaxian_geshu++; + //斑马线色块宽度清零，进行下一个黑色块的扫描计算 + banmaxian_kuandu = 0; + } + else + { + //如果不满足对黑色块的认为要求就直接清零，去计算下一个黑色块 + banmaxian_kuandu = 0; + } + } + } + //如果色块的个数在6~9之间则认为这一行的斑马线满足要求，在去扫下一行 + if (banmaxian_geshu >= 2 ){banmaxian_hangshu++;} } - } - - if (change_num > 14) { - garage_type = GARAGE_FOUND; - // printf("跳变点的数量为：%d\r\n", change_num); - } - - change_num = 0; + //如果有大于等于4行的有效斑马线 + if(banmaxian_hangshu>=2) + { + //斑马线标准位置1 + garage_type = GARAGE_FOUND; + } + else{garage_type = GARAGE_NONE;} } void RunGarage() { - if (garage_type == GARAGE_FOUND) { printf("识别到车库\r\n"); - garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题 + //garage_type = GARAGE_NONE; // TFIXME 原来是 garage_type == GARAGE_NONE，确认更改后无问题 } } \ No newline at end of file diff --git a/app/gl_garage.h b/app/gl_garage.h index 947a4d4..3524a8e 100644 --- a/app/gl_garage.h +++ b/app/gl_garage.h @@ -5,15 +5,6 @@ enum garage_type_e { GARAGE_NONE, // 非车库模式 GARAGE_FOUND, - GARAGE_OUT_LEFT, - GARAGE_OUT_RIGHT, // 出库，陀螺仪转过45°，即出库完毕 - GARAGE_FOUND_LEFT, - GARAGE_FOUND_RIGHT, // 发现车库，即斑马线+单侧L角点(未使用) - GARAGE_IN_LEFT, - GARAGE_IN_RIGHT, // 进库，发现车库后判断第几次，从而决定是否进库 - GARAGE_PASS_LEFT, - GARAGE_PASS_RIGHT, // 不进库，发现车库后判断第几次，从而决定是否进库 - GARAGE_STOP // 进库完毕，停车 }; extern enum garage_type_e garage_type; diff --git a/app/gl_get_corners.c b/app/gl_get_corners.c index c2339bb..c7b3e56 100644 --- a/app/gl_get_corners.c +++ b/app/gl_get_corners.c @@ -8,13 +8,13 @@ void get_corners() Lpt_in0_found = Lpt_in1_found = false; is_straight0 = pts_resample_left_count > 1.0 / RESAMPLEDIST; is_straight1 = pts_resample_right_count > 1.0 / RESAMPLEDIST; - is_turn0 = is_turn1 = 0; + // is_turn0_l = is_turn1_l = is_turn0_r = is_turn1_r = 0; for (int i = 0; i < pts_resample_left_count; i++) { if (angle_new_left[i] == 0) continue; int im1 = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1); int ip1 = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_left_count - 1); float conf = fabs(angle_left[i]) - (fabs(angle_left[im1]) + fabs(angle_left[ip1])) / 2; - ips114_show_float(120, 80, conf, 3, 4); + // ips114_show_float(120, 80, conf, 3, 4); // L 角点阈值 if (Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { Lpt0_rpts0s_id = i; @@ -22,16 +22,28 @@ void get_corners() // transform(pts_resample_left[Lpt0_rpts0s_id][1],pts_resample_left[Lpt0_rpts0s_id][0],&Lpt0[1],&Lpt0[0]); //待优化，是否用到，无用则删 } // 长直道阈值 - if (conf > (6. / 180. * PI32) && i < 1.2 / RESAMPLEDIST) { + if (conf > (6. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) { is_straight0 = false; } if (Lpt0_found == true && is_straight0 == false) break; } - if (Lpt0_found == false) - { - is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05); - } + // if (Lpt0_found == false) + // { + // // is_turn0 = is_curve(angle_left ,clip(angle_left_num - 10, 0,angle_left_num),0.05); + // int gap = 0; + // gap = pts_resample_left[clip(pts_resample_left_count - 30, 0, pts_resample_left_count - 1)][1] - pts_resample_left[0][1]; + // ips200_show_uint(200, 205, gap, 3); + // if (gap <= -10) + // { + // is_turn0_l = true; + // } + // if (gap >= 10) + // { + // is_turn1_l = true; + // } + + // } //if (is_turn0) //{ //state_type = TURN_LEFT_STATE; @@ -67,7 +79,7 @@ void get_corners() int im1 = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1); int ip1 = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_resample_right_count - 1); float conf = fabs(angle_right[i]) - (fabs(angle_right[im1]) + fabs(angle_right[ip1])) / 2; - ips114_show_float(100, 100, conf, 3, 4); + // ips114_show_float(100, 100, conf, 3, 4); if (Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { Lpt1_rpts1s_id = i; @@ -75,18 +87,29 @@ void get_corners() // transform(pts_resample_right[Lpt1_rpts1s_id][1],pts_resample_right[Lpt1_rpts1s_id][0],&Lpt1[1],&Lpt1[0]); } - if (conf > (6. / 180. * PI32) && i < 1.2 / RESAMPLEDIST) is_straight1 = false; + if (conf > (6. / 180. * PI32) && i < 0.8 / RESAMPLEDIST) is_straight1 = false; if (Lpt1_found == true && is_straight1 == false) break; } - if(is_straight1 && is_straight0){ - state_type = STRAIGHT_STATE; - } + // if(is_straight1 && is_straight0){ + // state_type = STRAIGHT_STATE; + // } - if (Lpt1_found == false) - { - is_turn1 = is_curve(angle_right ,clip(angle_right_num - 10, 0,angle_right_num),0.05); - } + // if (Lpt1_found == false) + // { + // // is_turn1 = is_curve(angle_right ,clip(angle_right_num - 10, 0,angle_right_num),0.05); + // int gap = 0; + // gap = pts_resample_right[clip(pts_resample_right_count - 30, 0, pts_resample_right_count - 1)][1] - pts_resample_right[0][1]; + // ips200_show_uint(200, 224, gap, 3); + // if (gap <= -10) + // { + // is_turn1_l = true; + // } + // if (gap >= 10) + // { + // is_turn1_r = true; + // } + // } for (int i = 0; i < pts_resample_right_count; i++) { if (angle_new_right_barrier[i] == 0) continue; @@ -108,8 +131,12 @@ void get_corners() } } - if (is_turn1 && is_turn0) - { - state_type = TURN_LEFT_STATE; - } + // if (is_turn0_l && is_turn1_l) + // { + // state_type = TURN_LEFT_STATE; + // } + // if (is_turn0_r && is_turn1_r) + // { + // state_type = TURN_RIGHT_STATE; + // } } diff --git a/app/gl_state.h b/app/gl_state.h index ecfa349..1ffd9bb 100644 --- a/app/gl_state.h +++ b/app/gl_state.h @@ -3,8 +3,7 @@ enum state_type_e { COMMON_STATE, - TURN_LEFT_STATE, - TURN_RIGHT_STATE, + TURN_STATE, STRAIGHT_STATE, }; diff --git a/app/gl_tracking.c b/app/gl_tracking.c index fae8979..6e4e8c9 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -124,6 +124,7 @@ void MidLineTrack() mid_track[begin_id][1] = cx; rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]); GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER); + // 远预锚点位置- int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1); @@ -131,6 +132,33 @@ void MidLineTrack() // 近锚点位置 int aim_idx_near = clip(round(0.09 / RESAMPLEDIST), 0, rptsn_num - 1); + int gap_1 = fabs(rptsn[3 * (rptsn_num / 4)][1] - rptsn[0][1]); + float dx1 = rptsn[3 * (rptsn_num / 4)][0] - rptsn[aim_idx][0]; + float dy1 = rptsn[3 * (rptsn_num / 4)][1] - rptsn[aim_idx][1]; + float dn1 = Q_sqrt(dx1 * dx1 + dy1 * dy1); + float dx2 = rptsn[aim_idx][0] - rptsn[aim_idx_near][0]; + float dy2 = rptsn[aim_idx][1] - rptsn[aim_idx_near][1]; + float dn2 = Q_sqrt(dx2 * dx2 + dy2 * dy2); + float c1 = dx1 / dn1; + float s1 = dy1 / dn1; + float c2 = dx2 / dn2; + float s2 = dy2 / dn2; + float angle_1= atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1); + ips114_show_float(120, 80, angle_1, 3, 4); + if (-0.3 <= angle_1 <= 0.3 ) + { + state_type = TURN_STATE; + } + else + { + state_type = STRAIGHT_STATE; + } + if (circle_type == CIRCLE_LEFT_IN && circle_type == CIRCLE_LEFT_OUT && circle_type == CIRCLE_RIGHT_IN && circle_type == CIRCLE_RIGHT_OUT) + { + state_type = TURN_STATE; + } + + // 计算远锚点偏差值 float dx = rptsn[aim_idx][1] - cx; float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER; diff --git a/app/page/page_rtcam.c b/app/page/page_rtcam.c index baf14c4..8d8bf57 100644 --- a/app/page/page_rtcam.c +++ b/app/page/page_rtcam.c @@ -53,8 +53,8 @@ static void Exit() static void Loop() { Show_Marked_Image(); - ips200_show_uint(60, 165, pts_inv_l_count, 3); - ips200_show_uint(60, 185, pts_inv_r_count, 3); + ips200_show_uint(60, 165, pts_resample_left_count, 3); + ips200_show_uint(60, 185, pts_resample_right_count, 3); ips200_show_uint(60, 205, is_straight0, 3); ips200_show_uint(60, 224, is_straight1, 3); ips200_show_uint(160, 165, Lpt0_found_barrier, 3);