From 2cab70791e4bf031e90a4b04e57e6d684597569e Mon Sep 17 00:00:00 2001 From: spdis Date: Mon, 29 Jun 2026 18:38:14 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=91=E9=A9=AC=E7=BA=BF:=203f=E5=8E=BB?= =?UTF-8?q?=E6=8A=96/-1=E8=A1=B0=E5=87=8F/3s=E5=81=9C=E8=BD=A6/=E8=B5=9B?= =?UTF-8?q?=E9=81=93=E5=A4=96=E5=BF=BD=E7=95=A5/=E4=B8=A2=E7=BA=BF?= =?UTF-8?q?=E5=BF=BD=E7=95=A5/reject=E6=97=A5=E5=BF=97;=20=E8=B5=B7?= =?UTF-8?q?=E6=AD=A5=E5=BC=B9=E5=B0=842x/0.7s;=20=E6=96=91=E9=A9=AC?= =?UTF-8?q?=E7=BA=BF=E5=90=8Edelay1.5s=E6=94=B9foresee=3D35/5s;=20PWM?= =?UTF-8?q?=E6=9E=84=E9=80=A0=E5=85=88=E5=BD=92=E9=9B=B6=E9=98=B2=E7=9E=AC?= =?UTF-8?q?=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/MotorController.cpp | 1 + src/camera.cpp | 92 ++++++++++++++++++++++++++++++++++++----- 2 files changed, 82 insertions(+), 11 deletions(-) diff --git a/src/MotorController.cpp b/src/MotorController.cpp index edf6153..e78c469 100644 --- a/src/MotorController.cpp +++ b/src/MotorController.cpp @@ -7,6 +7,7 @@ MotorController::MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned : pwmController(pwmchip, pwmnum), directionGPIO(gpioNum) { pwmController.setPeriod(period_ns); + pwmController.setDutyCycle(0); // 先归零再使能, 防止 sysfs 残留值导致电机瞬动 directionGPIO.setDirection("out"); pwmController.enable(); } diff --git a/src/camera.cpp b/src/camera.cpp index 84d21cb..f3ac3cf 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -86,7 +86,7 @@ static int g_box_count = 0; // g_zc_min_cy: 本轮追踪中斑马线出现时的最小 cy (越小=越远) // g_ztime: 进入 Z_STOP / Z_COOLDOWN 的时间戳 // ═══════════════════════════════════════════════════════════ -static constexpr int ZEBRA_MIN_FRAMES = 5; // 连续确认帧数 +static constexpr int ZEBRA_MIN_FRAMES = 3; // 连续确认帧数 static constexpr int ZEBRA_FAR_CY = 80; // 远处 cy 阈值: 斑马线必须曾出现在 cy≤80 static int g_zc_frames = 0; static int g_zc_min_cy = 120; // 初始设远 (120=从未检测到) @@ -426,6 +426,12 @@ static void run_model_inference() // ═══════════════════════════════════════════════════════════ static bool zebra_process() { + // 双边丢线超过50% → 不采纳任何斑马线检测 + if (g_lost_rows > line_tracking_height / 2) { + if (g_cfg.debug) printf("[ZEBRA] 忽略: 丢线%d/%d行\n", g_lost_rows, line_tracking_height); + return false; + } + bool zebra_seen = false; bool zebra_near = false; int zebra_cy = 0; @@ -434,6 +440,30 @@ static bool zebra_process() // 遍历检测框, 找第一个斑马线 for (int i = 0; i < g_box_count; ++i) { if (g_boxes[i].cls != MD_ZEBRA) continue; + // ── 赛道内占比检查: 框80%以上在赛道外 → 忽略 ── + { + float bx = (float)line_tracking_width / raw_frame.cols; + float by = (float)line_tracking_height / raw_frame.rows; + int bx1 = (int)((g_boxes[i].cx - g_boxes[i].w/2) * bx); + int by1 = (int)((g_boxes[i].cy - g_boxes[i].h/2) * by); + int bx2 = (int)((g_boxes[i].cx + g_boxes[i].w/2) * bx); + int by2 = (int)((g_boxes[i].cy + g_boxes[i].h/2) * by); + int overlap = 0, area = (bx2 - bx1) * (by2 - by1); + if (area > 0) { + int y0 = std::max(0, std::min(by1, line_tracking_height - 1)); + int y1 = std::min(by2, line_tracking_height - 1); + for (int y = y0; y <= y1 && y >= 0; ++y) { + if (left_line[y] == -1 || right_line[y] == -1) continue; + int ol = std::max(0, std::min(bx2, right_line[y]) - std::max(bx1, left_line[y])); + overlap += ol; + } + } + if (area > 0 && (float)overlap / area < 0.2f) { + if (g_cfg.debug) printf("[ZEBRA] 忽略: 赛道内仅%.0f%% cf=%.2f\n", + (float)overlap / area * 100, g_boxes[i].conf); + continue; // 赛道内<20% → 忽略 + } + } zebra_cy = (int)g_boxes[i].cy; zebra_cf = g_boxes[i].conf; zebra_seen = true; @@ -447,7 +477,7 @@ static bool zebra_process() // 未检测到 → 衰减去抖计数 if (!zebra_seen) { - if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 2); // 每帧 -2 快速衰减 + if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 1); // 每帧 -1 衰减 if (g_zc_frames == 0) g_zc_min_cy = 120; // 完全衰减 → 重置远处追踪 } @@ -458,12 +488,19 @@ static bool zebra_process() if (zebra_near && g_zc_frames >= ZEBRA_MIN_FRAMES && g_zc_min_cy <= ZEBRA_FAR_CY) { play_zebra_audio(); // I2C 语音播报 g_zstate = Z_STOP; g_ztime = now; - if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车4s 冷却5s\n", zebra_cy, zebra_cf); + if (g_cfg.debug) printf("[ZEBRA] cy=%d cf=%.2f 停车3s 冷却5s\n", zebra_cy, zebra_cf); g_zc_frames = 0; g_zc_min_cy = 120; // 重置, 为下次检测做准备 + } else if (zebra_seen && g_cfg.debug) { + if (!zebra_near) + printf("[ZEBRA] 忽略: 太远 cy=%d <= %d\n", zebra_cy, (int)g_cfg.zebrasee); + else if (g_zc_frames < ZEBRA_MIN_FRAMES) + printf("[ZEBRA] 忽略: 帧数不足 %d/%d cy=%d\n", g_zc_frames, ZEBRA_MIN_FRAMES, zebra_cy); + else if (g_zc_min_cy > ZEBRA_FAR_CY) + printf("[ZEBRA] 忽略: 未曾远距出现 min_cy=%d > %d\n", g_zc_min_cy, ZEBRA_FAR_CY); } break; case Z_STOP: - if (now - g_ztime >= 4) { // 停车 4 秒 + if (now - g_ztime >= 3) { // 停车 3 秒 g_zstate = Z_COOLDOWN; g_ztime = now; if (g_cfg.debug) printf("[ZEBRA] 起步\n"); } @@ -863,12 +900,12 @@ static void detect_sharp_turn() static void steering_update() { if (!g_cfg.start) return; - // foresee (显示空间像素) → check_row (巡线空间行号) - // double scale = 1.0; - // if (g_sharp_turn) scale = g_cfg.sharp_turn_scale; - // else if (g_lost_rows > 15) scale = g_cfg.foresee_lost_scale; - // double eff_foresee = g_cfg.foresee * scale; - int check_row = (int)g_cfg.foresee / calc_scale; + // 斑马线起步后延迟1.5s, 再5s内缩短前瞻, 先走稳再改 + double eff_foresee = g_cfg.foresee; + if (g_zstate == Z_COOLDOWN && time(nullptr) - g_ztime >= 2 + && time(nullptr) - g_ztime < 5) + eff_foresee = 35; + int check_row = (int)eff_foresee / calc_scale; if (check_row < 0 || check_row >= line_tracking_height) return; if (mid_line[check_row] == -1) return; // 丢线行 → 跳过 @@ -904,10 +941,43 @@ static void steering_update() // 调用 ControlUpdate(final_spd, block) 执行: // block = zebra_block || tl_block → 编码器比例刹车 // 否则 → 开环占空比驱动 +// +// 起步弹射: 启动/斑马线恢复后 1.5x 速度持续 0.5s 快速起步 // ═══════════════════════════════════════════════════════════ +static struct timespec g_boost_end = {0, 0}; +static double g_boost_mul = 2.0; +static constexpr double BOOST_SECS = 0.7; + +static void trigger_boost() +{ + clock_gettime(CLOCK_MONOTONIC, &g_boost_end); + g_boost_end.tv_nsec += (long)(BOOST_SECS * 1e9); + if (g_boost_end.tv_nsec >= 1000000000L) { + g_boost_end.tv_sec += 1; + g_boost_end.tv_nsec -= 1000000000L; + } +} + +static double boost_factor() +{ + struct timespec now; + clock_gettime(CLOCK_MONOTONIC, &now); + if (now.tv_sec > g_boost_end.tv_sec) return 1.0; + if (now.tv_sec == g_boost_end.tv_sec && now.tv_nsec >= g_boost_end.tv_nsec) return 1.0; + return g_boost_mul; +} + static void motor_update(bool zebra_block, bool tl_block) { - double spd = target_speed; + // 起步弹射检测 + static int prev_start = 0; + static ZState prev_zstate = Z_NORMAL; + if (g_cfg.start && !prev_start) trigger_boost(); // 启动 + if (prev_zstate == Z_STOP && g_zstate == Z_COOLDOWN) trigger_boost(); // 斑马线起步 + prev_start = g_cfg.start; + prev_zstate = g_zstate; + + double spd = target_speed * boost_factor(); // 弯道减速: 舵机偏差越大 → 速度越低 double curve = 1.0 - std::abs(g_steer_deviation) * g_cfg.curve_slope;