diff --git a/app/gl_barrier.c b/app/gl_barrier.c index b616572..8ad72a7 100644 --- a/app/gl_barrier.c +++ b/app/gl_barrier.c @@ -20,7 +20,7 @@ void RunBarrier() if (Lpt0_found) { Lpt0_found_count++; } - if (Lpt0_found_count >= 1 && time_barrier >= 5000) { + if (Lpt0_found_count >= 1) { Lpt0_found_count = 0; barrier_type = BARRIER_LEFT_RUNNING; track_type = TRACK_RIGHT; @@ -30,7 +30,7 @@ void RunBarrier() } else if (barrier_type == BARRIER_LEFT_RUNNING) { track_type = TRACK_RIGHT; time_barrier = timer_get(TIM_3); - if (time_barrier >= 5000) { + if (time_barrier >= 1000) { barrier_type = BARRIER_NONE; track_type = TRACK_RIGHT; timer_start(TIM_3); @@ -53,7 +53,7 @@ void RunBarrier() } else if (barrier_type == BARRIER_RIGHT_RUNNING) { track_type = TRACK_LEFT; time_barrier = timer_get(TIM_3); - if (time_barrier >= 5000) { + if (time_barrier >= 1000) { timer_start(TIM_3); timer_clear(TIM_3); barrier_type = BARRIER_NONE; diff --git a/app/gl_garage.c b/app/gl_garage.c index 16114a6..afd144a 100644 --- a/app/gl_garage.c +++ b/app/gl_garage.c @@ -88,7 +88,7 @@ void CheckGarage() } } //如果色块的个数在6~9之间则认为这一行的斑马线满足要求，在去扫下一行 - if (banmaxian_geshu >= 2 ){banmaxian_hangshu++;} + if (banmaxian_geshu >= 6 ){banmaxian_hangshu++;} } //如果有大于等于4行的有效斑马线 if(banmaxian_hangshu>=2) diff --git a/app/gl_state.h b/app/gl_state.h index 1ffd9bb..26e1dcf 100644 --- a/app/gl_state.h +++ b/app/gl_state.h @@ -5,6 +5,7 @@ enum state_type_e { COMMON_STATE, TURN_STATE, STRAIGHT_STATE, + }; extern enum state_type_e state_type; diff --git a/app/gl_tracking.c b/app/gl_tracking.c index e0f6464..eca0aa7 100644 --- a/app/gl_tracking.c +++ b/app/gl_tracking.c @@ -1,6 +1,6 @@ #include "zf_common_headfile.h" #include "gl_headfile.h" - +#include "jj_blueteeth.h" float (*mid_track)[2]; int32_t mid_track_count; float pure_angle; @@ -124,7 +124,6 @@ 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); @@ -132,41 +131,37 @@ 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); + 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 ) - { + if (angle_1 >= 0.3f || angle_1 <= -0.3f) { state_type = TURN_STATE; - } - else - { + } 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; - } - + // 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) + //{ + // state_type = TURN_STATE; + //} // 计算远锚点偏差值 float dx = rptsn[aim_idx][1] - cx; float dy = cy - rptsn[aim_idx][0] + 0.2f * PIXPERMETER; float dn = (float)Q_sqrt(dx * dx + dy * dy); // float error = -atan2f(dx, dy) * 180 / PI32; - + bt_printf("%d", barrier_type); if (barrier_type == BARRIER_LEFT_BEGIN) { - dx_near = rptsn[aim_idx_near][1] - cx +barrier_offset; + dx_near = rptsn[aim_idx_near][1] - cx + barrier_offset; pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; } else if (barrier_type == BARRIER_RIGHT_BEGIN) { dx_near = rptsn[aim_idx_near][1] - cx - barrier_offset; @@ -187,4 +182,7 @@ void MidLineTrack() // // //考虑远点 // pure_angle = -atanf(PIXPERMETER * 2.0f * 0.2f * dx / dn / dn) / PI32 * 180.0f; } + 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) { + state_type = TURN_STATE; + } } diff --git a/app/jj_blueteeth.c b/app/jj_blueteeth.c index 62a0efc..751e1a4 100644 --- a/app/jj_blueteeth.c +++ b/app/jj_blueteeth.c @@ -21,7 +21,7 @@ void jj_bt_run() { } -void jj_bt_printf(const char *format, ...) +void bt_printf(const char *format, ...) { char sbuf[40]; va_list args; diff --git a/app/jj_blueteeth.h b/app/jj_blueteeth.h index 2e6b419..0747fbb 100644 --- a/app/jj_blueteeth.h +++ b/app/jj_blueteeth.h @@ -10,5 +10,5 @@ void jj_bt_init(); void jj_bt_run(); -void jj_bt_printf(const char *format, ...); +void bt_printf(const char *format, ...); #endif \ No newline at end of file diff --git a/app/main.c b/app/main.c index b3cd663..c68d919 100644 --- a/app/main.c +++ b/app/main.c @@ -59,7 +59,7 @@ int main(void) tiny_frame_param[0].f32 = pure_angle; tiny_frame_param[1].f32 = dx_near; tiny_frame_param[2].u8[0] = (uint8_t)state_type; // 0: 无状态 1: 弯道 2: 直行 - tiny_frame_param[3].u8[1] = (uint8_t)garage_type; // 0: 无车库 1: 有车库 + 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)));