diff --git a/app/gl_barrier.c b/app/gl_barrier.c index d2aada9..d2bc36f 100644 --- a/app/gl_barrier.c +++ b/app/gl_barrier.c @@ -2,7 +2,7 @@ #include "gl_headfile.h" enum barrier_type_e barrier_type = BARRIER_NONE; -enum s_type_e s_type = S_NONE; +enum s_type_e s_type = S_NONE; uint16 time_barrier; float (*mid_track_s)[2]; int32_t mid_track_count_s; @@ -24,32 +24,27 @@ void RunBarrier() { if (barrier_type == BARRIER_LEFT_BEGIN) { track_type = TRACK_RIGHT; - if (barrier_begin_flag == 0) - { + if (barrier_begin_flag == 0) { timer_clear(TIM_3); timer_start(TIM_3); barrier_begin_flag = 1; } - - if (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 >= 1000 && time_barrier < 2000) - { + 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); } - else if (time_barrier >= 2000) - { - barrier_type = BARRIER_NONE; - barrier_begin_flag = 0; - timer_stop(TIM_3); - timer_clear(TIM_3); - } - } - - + } + // if (Lpt0_found) { // Lpt0_found_count++; // } @@ -69,35 +64,29 @@ void RunBarrier() // timer_start(TIM_3); // timer_clear(TIM_3); // } - } if (barrier_type == BARRIER_RIGHT_BEGIN) { track_type = TRACK_LEFT; - if (barrier_begin_flag == 0) - { + if (barrier_begin_flag == 0) { timer_clear(TIM_3); timer_start(TIM_3); barrier_begin_flag = 1; } - - if (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 >= 1000) - { + if (time_barrier >= 300 && time_barrier < 600) { barrier_type = BARRIER_RIGHT_RUNNING; - if (time_barrier >= 2000) - { - barrier_type = BARRIER_NONE; - barrier_begin_flag = 0; - timer_stop(TIM_3); - timer_clear(TIM_3); - } - + } else if (time_barrier >= 600) { + barrier_type = BARRIER_NONE; + barrier_begin_flag = 0; + timer_stop(TIM_3); + timer_clear(TIM_3); } - } + } // if (Lpt1_found) { // Lpt1_found_count++; @@ -118,11 +107,10 @@ void RunBarrier() // barrier_type = BARRIER_NONE; // track_type = TRACK_LEFT; // } - } } - -void Check_s(){ +void Check_s() +{ if (track_type == TRACK_LEFT) { mid_track_s = mid_left; mid_track_count_s = mid_left_count; @@ -134,35 +122,26 @@ void Check_s(){ int min_s = 1000; int max_s = 1; - for (int i = 0; i < mid_track_count_s; i++) - { - if (mid_track_s[i][1] < min_s) - { - min_s = mid_track_s[i][1]; + for (int i = 0; i < mid_track_count_s; i++) { + if (mid_track_s[i][1] < min_s) { + min_s = mid_track_s[i][1]; temp_min_s = min_s; } - if (mid_track_s[i][1] > max_s) - { - max_s = mid_track_s[i][1]; + if (mid_track_s[i][1] > max_s) { + max_s = mid_track_s[i][1]; temp_max_s = max_s; } - } - - if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5) - { + if (is_straight0 == false && is_straight1 == false && (max_s - min_s) <= 30 && (max_s - min_s) >= 5) { s_type = S_BEGIN; } - } +void RunS() +{ -void RunS(){ - - if (temp_max_s - temp_min_s >30 || (is_straight0 && is_straight1)) - { + if (temp_max_s - temp_min_s > 30 || (is_straight0 && is_straight1)) { s_type = S_NONE; } - } \ No newline at end of file diff --git a/app/gl_get_corners.c b/app/gl_get_corners.c index 7414e17..8051a61 100644 --- a/app/gl_get_corners.c +++ b/app/gl_get_corners.c @@ -59,7 +59,7 @@ void get_corners() float conf = fabs(angle_left_barrier[i]) - (fabs(angle_left_barrier[im1]) + fabs(angle_left_barrier[ip1])) / 2; // L 角点阈值 - if (Lpt0_found_barrier == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.5 / RESAMPLEDIST) { + if (Lpt0_found_barrier == false && (40. / 180. * PI32) < conf && conf < (160. / 180. * PI32) && i < 0.6 / RESAMPLEDIST) { Lpt0_rpts0s_id_barrier = i; Lpt0_found_barrier = true; } @@ -116,7 +116,7 @@ void get_corners() int ip1 = clip(i + (int)round(ANGLEDIST_barrier / RESAMPLEDIST), 0, pts_resample_right_count - 1); float conf = fabs(angle_right_barrier[i]) - (fabs(angle_right_barrier[im1]) + fabs(angle_right_barrier[ip1])) / 2; - if (Lpt1_found_barrier == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.5 / RESAMPLEDIST) { + if (Lpt1_found_barrier == false && (40. / 180. * PI32) < conf && conf < 160. / 180. * PI32 && i < 0.6 / RESAMPLEDIST) { Lpt1_rpts1s_id_barrier = i; Lpt1_found_barrier = true; } diff --git a/app/gl_tracking.c b/app/gl_tracking.c index 9dba0d7..82ad5b6 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -14,7 +14,7 @@ float (*rpts)[2]; int rpts_num; float last_pure_angle = 0.0f; int8_t turn_flag = 0; -int circle_flag = 1; +int circle_flag = 1; // 计算最小二乘法斜率的函数 float leastSquaresSlope(float points[][2], int n) { @@ -270,31 +270,55 @@ void MidLineTrack() float dy = cy - rptsn[aim_idx][0]; // + 0.2f * PIXPERMETER; float dn = (dx * dx + dy * dy); float temp_near = 0; - 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; - } + + switch (barrier_type) { + case BARRIER_LEFT_BEGIN: + // dx_near = mid_track[aim_idx_near][1] - cx + barrier_offset; + pure_angle = -45.f; + break; + case BARRIER_LEFT_RUNNING: + pure_angle = 25.f; + break; + case BARRIER_RIGHT_BEGIN: + pure_angle = 45.f; + break; + case BARRIER_RIGHT_RUNNING: + // dx_near = mid_track[aim_idx_near][1] - cx - barrier_offset; + pure_angle = -25.f; + default: + // 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 (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; + // } + // } + // // 计算近锚点偏差值 // dx_near = rptsn[aim_idx_near][1] - cx; // // float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER; @@ -308,32 +332,25 @@ void MidLineTrack() 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_flag == 1) - { + if (circle_flag == 1) { state_type = CIRCLE_STATE; } - + uint16 ti = timer_get(TIM_3); if (ti >= 1000) { timer_stop(TIM_3); timer_clear(TIM_3); - + circle_flag = 0; } - if (circle_flag == 0) - { + if (circle_flag == 0) { state_type = CIRCLE_RUNNING_STATE; } - } - if (circle_type == CIRCLE_NONE) - { + if (circle_type == CIRCLE_NONE) { circle_flag = 1; } - - - 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; diff --git a/app/main.c b/app/main.c index ff5d22a..0b27825 100644 --- a/app/main.c +++ b/app/main.c @@ -56,10 +56,10 @@ int main(void) while (1) { Page_Run(); by_buzzer_run(); - tiny_frame_param[0].f32 = pure_angle; - tiny_frame_param[1].f32 = curvature; - tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行 - tiny_frame_param[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库 + tiny_frame_param[0].f32 = pure_angle; + tiny_frame_param[1].f32 = curvature; + tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行 + tiny_frame_param[2].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库 if (mt9v03x_finish_flag) { // 该操作消耗大概 1970 个 tick，折合约 110us memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); @@ -73,7 +73,9 @@ int main(void) ElementRun(); aim_distance_select(); MidLineTrack(); - //bt_printf("type= %d", (int)curvature); + // if (barrier_type) { + // bt_printf("type= %d", barrier_type); + // } } } }