spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

模型推理移至管道末尾降舵机延迟60→16ms + 视觉Mat复用 + 端到端延迟计时 + OpenCV 4.13 + WASD遥控

Browse Source
This commit is contained in:
spdis
2026-06-29 16:43:41 +08:00
parent 40d5cb1604
commit 017a827b18
16 changed files with 1201 additions and 709 deletions
Download Patch File Download Diff File Expand all files Collapse all files
AGENTS.md
+54 -88
View File
@@ -3,7 +3,7 @@
## Project overview ## Project overview
龙芯 2K0300 嵌入式自动驾驶智能车，OpenCV + Mild v12 魔改 YOLO 模型。 龙芯 2K0300 嵌入式自动驾驶智能车，OpenCV + Mild v12 魔改 YOLO 模型。
板端运行，x86 Linux 交叉编译。 板端运行，x86 Linux 交叉编译。No unit tests, no CI, no linting — tested on-device only.
## Build ## Build
@@ -17,30 +17,33 @@ make -j$(nproc)
- **Cross-compilation target**: LoongArch64 (`-march=loongarch64 -mtune=loongarch64`) - **Cross-compilation target**: LoongArch64 (`-march=loongarch64 -mtune=loongarch64`)
- **Toolchain**: `/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6` - **Toolchain**: `/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6`
- **C++ standard**: 17 - **C++ standard**: 17
- **OpenCV path** (device-side): `D:\PPPProgram\smartcar\opencv_device\` (mounted as `/mnt/d/...` in WSL) - **OpenCV path** (WSL): `/home/ilikara/loongson/opencv-4.13.0/loongson` (v4.13.0, 无 SIMD)
- **OpenCV 设备库**: `opencv_libs/` 目录下 `.so.413` 文件，部署到设备 `/home/root/opencv/lib/` 并更新 `.so` 符号链接
- `cross.cmake` is included **before** `cmake_minimum_required` — intentional, to set compiler before project(). - `cross.cmake` is included **before** `cmake_minimum_required` — intentional, to set compiler before project().
- Set `CROSS_COMPILE` to `0` in `cross.cmake` for native x86 builds.
## Project structure ## Project structure
| Directory | Purpose | | Directory | Purpose |
|-----------|---------| |-----------|---------|
| `src/` | Source → compiled into `common_lib` (static lib) | | `src/` | Source (.cpp + .hpp) → compiled into `common_lib` (static lib) |
| `lib/` | Public headers only (no .cpp) | | `lib/` | Public headers (.h only) + `vl53l0x/` C API sources |
| `main/` | Entry point → executable `smartcar_demo1` | | `main/` | Entry points → `smartcar_demo1` (main) + `lidar_test` (standalone test) |
| `docs/` | Architecture docs (`ARCHITECTURE.md` is the design reference) | | `docs/` | Design docs (ARCHITECTURE, CONE_DESIGN, ENCODER_BRAKE, LIDAR_AVOID, TRAFFIC_LIGHT) |
| `build/` | CMake build output (gitignored) | | `build/` | CMake build output (gitignored) |
**Executable name**: `smartcar_demo1` (CMake project name `smartcar_demo2` ≠ executable name). **Executable name**: `smartcar_demo1` (CMake project name is `smartcar_demo2` — mismatch is intentional).
## Excluded modules ## Excluded modules
These source files exist but are **excluded from build**: These source files exist in `src/` but are **excluded from build** via CMakeLists.txt `FILTER EXCLUDE`:
- `vl53l0x.cpp` — laser ranging module, hardware not connected
- `zebra_detect.cpp` — classic zebra-crossing detection (replaced by Mild model) - `zebra_detect.cpp` — classic zebra-crossing detection (replaced by Mild model)
- `PIDController.cpp` — unused (motor is open-loop direct drive) - `PIDController.cpp` — unused (motor is open-loop direct drive)
- `serial.cpp` — Vofa/serial image streaming, unused - `serial.cpp` — Vofa/serial image streaming, unused
Note: `vl53l0x.cpp` IS compiled (lidar ranging is active).
## Device-side operation ## Device-side operation
Controlled via `ctl.sh` (writes to text files, no CLI args): Controlled via `ctl.sh` (writes to text files, no CLI args):
@@ -48,106 +51,69 @@ Controlled via `ctl.sh` (writes to text files, no CLI args):
```sh ```sh
# On device: # On device:
sh ctl.sh init # initialize GPIO/PWM pins + write default config files sh ctl.sh init # initialize GPIO/PWM pins + write default config files
sh ctl.sh start # start the demo in background sh ctl.sh start # init_pins + start the demo in background
sh ctl.sh stop # stop and zero PWM sh ctl.sh stop # stop and zero PWM
``` ```
`ctl.sh` sets default config values by writing to files like `./speed`, `./kp`, `./steer_gain`, etc. The running binary reads these files at startup and optionally re-reads them every 7 frames (when `./debug` = 1). `ctl.sh` writes default config values to text files (`./speed`, `./steer_gain`, etc.). The binary reads them at startup via `cfg_load_all()` and re-reads every 30 frames when `./debug` = 1.
`start.sh` is an alternative launcher using `LD_PRELOAD=./gpio_fix_final.so` for the GPIO 74 workaround. **Note**: `start.sh` references the binary as `smartcar_demo` but the actual executable is `smartcar_demo1` — this is a known bug, use `ctl.sh` instead. **Bug**: `start.sh` references binary as `smartcar_demo` but actual executable is `smartcar_demo1` — use `ctl.sh` instead.
## Configuration system ## Configuration system
All config is via single-value text files in the working directory: All config is via single-value text files in the working directory. The full list of parameters and defaults is in `lib/global.h` (struct `CfgCache`). Config is loaded by `cfg_load_all()` in `src/global.cpp`.
### Steering Key groups:
- `./speed` (double) — target speed (duty cycle %) - **Steering**: `speed`, `deadband`, `steer_gain`, `center_bias`, `foresee`
- `./deadband` — steering deadband (pixels) - **Debug**: `start` (0/1), `showImg` (0/1), `debug` (0/1), `destfps`, `saveImg`
- `./steer_gain` — steering gain multiplier - **Zebra**: `zebrasee`
- `./center_bias` — midline offset correction - **Cone avoidance**: `cone_avoid_gain`, `cone_avoid_range`, `cone_speed`, `cone_min_frames`, `cone_margin`, `cone_thresh`, `cone_hold_frames`, `cone_return_gain`, `cone_return_frames`
- `./foresee` — look-ahead row for steering - **Curve slowdown**: `curve_slope` (default 0.4), `curve_min` (default 0.8)
- **Encoder brake**: `brake_scale`, `brake_max`
### Debug / Display - **Lidar avoidance**: `lidar_enable`, `lidar_thresh`, `lidar_pre`, `lidar_near`, `lidar_far`, `lidar_near_start`, `lidar_near_end`, `lidar_far_span`, `lidar_min_frames`, `lidar_avoid_gain`, `lidar_avoid_range`, `lidar_speed`, `lidar_hold_frames`
- `./start` (0/1) — motor enable switch
- `./showImg` (0/1) — LCD display toggle
- `./debug` (0/1) — when 1, reloads all config files every 7 frames
- `./destfps` — target framerate
- `./saveImg` — when written as 1 (in debug mode), saves current frame
### Zebra crossing
- `./zebrasee` — zebra crossing near-threshold (cy > this = near)
### Cone avoidance
- `./cone_avoid_gain` — midline deformation push amount (normalized)
- `./cone_avoid_range` — deformation ramp steepness
- `./cone_speed` — speed multiplier when cone triggered
- `./cone_min_frames` — consecutive confirmation frames
- `./cone_margin` — 0=center point, 1=box edge
- `./cone_thresh` — confidence threshold
- `./cone_hold_frames` — hold deformation frames after cone disappears
### Encoder brake
- `./brake_scale` — speed (pps) → brake duty cycle (ns) scaling factor
- `./brake_max` — maximum brake duty cycle (ns)
### PID gains (reserved, motor is open-loop)
- `./kp`, `./ki`, `./kd` — steering PID (currently not used)
- `./mortor_kp`, `./mortor_ki`, `./mortor_kd` — motor encoder PID (note: spelling `mortor` is intentional)
## Architecture ## Architecture
### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:578`) ### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:755`)
CameraHandler 现已拆分为多个函数，主函数 ~40 行仅做编排： ```
```cpp
CameraHandler() CameraHandler()
├── capture_frame() // 1. MJPG 取帧 → 1/4解码 320×240 BGR ├── capture_frame() // 1. MJPG 取帧 → 1/4解码 320×240 BGR
├── save_image_if_requested() // 2. 保存帧 (debug) ├── save_image_if_requested() // 2. 保存帧 (debug)
├── image_main() // 3. 视觉巡线 (80×60 HSV-Otsu-FloodFill) ├── image_main() // 3. 视觉巡线 (80×60 HSV-Otsu-FloodFill)
├── run_model_inference() // 4. Mild v12 每2帧推理 (160×120, 4类) ├── lidar_avoid_process() // 4. VL53L0X 激光避障 (每2帧) + 中线变形
├── zebra_process() // 5a. 斑马线去抖 + Z_STOP(4s)/Z_COOLDOWN(5s) ├── run_model_inference() // 5. Mild v12 每2帧推理 (160×120, 4类)
├── traffic_light_process() // 5b. 红绿灯 TL_NORMAL→TL_STOP→TL_WAIT_GREEN ├── zebra_process() // 6a. 斑马线去抖 + Z_STOP/Z_COOLDOWN
├── cone_detect_and_deform() // 5c. 锥桶检测 + 中线变形 + 保持衰减 ├── traffic_light_process() // 6b. 红绿灯 TL_NORMAL→TL_STOP→TL_WAIT_GREEN
├── steering_update() // 6. 舵机比例控制 (deadband过滤) ├── cone_detect_and_deform() // 6c. 锥桶检测 + 中线变形 + 保持衰减
├── motor_update() // 7. 电机开环PWM + 编码器刹车 + 弯道减速 ├── steering_update() // 7. 舵机比例控制 (deadband过滤)
├── lcd_render() // 8. LCD RGB565渲染 (边界线+检测框+状态) ├── motor_update() // 8. 电机开环PWM + 编码器刹车 + 弯道减速
└── fps_log() // 9. 每15帧打印分步耗时 ├── lcd_render() // 9. LCD RGB565渲染 (边界线+检测框+状态)
└── fps_log() // 10. 分步耗时日志
``` ```
### Motor control details Also: `malloc_trim(0)` every 150 frames to return freed memory to OS (embedded optimization).
- **Open-loop**: `MotorController::updateduty(spd)` — direct PWM duty cycle, no encoder feedback in normal driving. ### Motor control
- **Encoder brake**: When `zebra_block=true` (red light or zebra stop), reads `g_enc_speed` from the encoder thread and applies reverse braking proportional to current speed.
- **Encoder thread** (`control.cpp:32`): polls GPIO67 (LSB pulse) + GPIO72 (direction) at ~2kHz, 100ms window speed calculation → `g_enc_speed` (pulses/sec). Now includes 500µs sleep to prevent CPU starvation. - **Open-loop**: `MotorController::updateduty(spd)` — direct PWM, no encoder feedback in normal driving.
- **Curve slowdown**: `speed *= (1.0 - |deviation| × 0.4)`, clamped to ≥ 60%. - **Encoder brake**: When `zebra_block || tl_block`, reads `g_enc_speed` from encoder thread and applies reverse braking.
- **Encoder thread** (`control.cpp`): polls GPIO67 (pulse) + GPIO72 (direction), 100ms window → `g_enc_speed` (pulses/sec). 500µs sleep to prevent CPU starvation.
- **Curve slowdown**: `speed *= (1.0 - |deviation| × curve_slope)`, clamped to ≥ `curve_min`.
- **Cone/lidar speed reduction**: Code exists but is currently **commented out** in `motor_update()`.
### Model: Mild v12 ### Model: Mild v12
- **Architecture**: 3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9) - **Input**: 160×120 BGR, **Output**: 4 classes + background
- **Input**: 160×120 BGR - **Class indices**: 0=cone, 1=red light, 2=green light, 3=zebra
- **Output**: 4 classes (cone / red light / green light / zebra) + background - **Per-class thresholds**: `{0.65, 0.75, 0.80, 0.82}` (cone/red/green/zebra)
- **Classes used in decision logic**:
- cls=0 (cone) → midline deformation avoidance
- cls=1 (red light) → traffic light state machine stop
- cls=2 (green light) → traffic light state machine resume
- cls=3 (zebra) → zebra crossing state machine stop
- **Per-class thresholds**: `{0.90, 0.75, 0.80, 0.90}` (cone/red/green/zebra)
- **Weight file**: `mild_v12.bin` (105KB custom binary format) - **Weight file**: `mild_v12.bin` (105KB custom binary format)
- **Inference engine**: `src/model_v10.{hpp,cpp}` (names kept as `model_v10_*` for compatibility, internally Mild v3) - **Inference engine**: `src/model_v10.{hpp,cpp}` (file names say v10, internally Mild v3 — historical naming)
### Vision pipeline details
- Processing resolution: 80×60 (optimal balance for 2K0300 CPU)
- HSV dual-channel Otsu: H channel (blue hue) + S channel (saturation) → bitwise AND → track region
- FloodFill: seed at bottom center (40, 50), 8-connected, extracts connected track
- Per-row longest-run search on floodFill result → left/right boundaries
- Missing-line recovery: propagate downward midline + virtual boundaries
## Style and conventions ## Style and conventions
- C++ files have **no copyright headers** — comments are ascii-box-style block comments when present - No copyright headers — comments use ascii-box-style block dividers
- Header guards use `#ifndef FILENAME_H_` / `#define FILENAME_H_` format (with some exceptions: `#pragma once` in model files) - Header guards: `#ifndef FILENAME_H_` / `#define FILENAME_H_` (some model files use `#pragma once`)
- Global state uses `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`) - Global state via `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`)
- `lib/` contains .h files only; `src/` contains .cpp files only - `lib/` has `.h` headers; `src/` has `.cpp` implementations + `.hpp` model headers
- No unit tests, no CI, no linting — this is embedded code tested on-device - No unit tests, no CI, no linting — do not add comments to code unless asked
CMakeLists.txt
+2 -1
View File
@@ -7,13 +7,14 @@ cmake_minimum_required(VERSION 3.5.0)
# 设置 C++ 标准 # 设置 C++ 标准
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath,/home/root/opencv/lib -Wl,--disable-new-dtags")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall")
# 定义项目名称和版本 # 定义项目名称和版本
project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX) project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX)
# OpenCV 设备端路径 # OpenCV 设备端路径
set(OpenCV_DIR /mnt/d/PPPProgram/smartcar/opencv_device/lib/cmake/opencv4) set(OpenCV_DIR /home/ilikara/loongson/opencv-4.13.0/loongson/lib/cmake/opencv4)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS}) include_directories(${OpenCV_INCLUDE_DIRS})
message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}") message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
ctl.sh
+3 -1
View File
@@ -36,12 +36,14 @@ init_pins() {
echo 8 > "$DIR/deadband" 2>/dev/null echo 8 > "$DIR/deadband" 2>/dev/null
echo 1.5 > "$DIR/steer_gain" 2>/dev/null echo 1.5 > "$DIR/steer_gain" 2>/dev/null
echo 40 > "$DIR/foresee" 2>/dev/null echo 40 > "$DIR/foresee" 2>/dev/null
echo 0.7 > "$DIR/foresee_lost_scale" 2>/dev/null
echo 0.5 > "$DIR/sharp_turn_scale" 2>/dev/null
echo 0 > "$DIR/start" 2>/dev/null echo 0 > "$DIR/start" 2>/dev/null
echo 0.3 > "$DIR/cone_avoid_gain" 2>/dev/null echo 0.3 > "$DIR/cone_avoid_gain" 2>/dev/null
echo 30 > "$DIR/cone_avoid_range" 2>/dev/null echo 30 > "$DIR/cone_avoid_range" 2>/dev/null
echo 0.5 > "$DIR/cone_speed" 2>/dev/null echo 0.5 > "$DIR/cone_speed" 2>/dev/null
echo 3 > "$DIR/cone_min_frames" 2>/dev/null echo 1 > "$DIR/cone_min_frames" 2>/dev/null
echo 0 > "$DIR/cone_margin" 2>/dev/null echo 0 > "$DIR/cone_margin" 2>/dev/null
echo 0.80 > "$DIR/cone_thresh" 2>/dev/null echo 0.80 > "$DIR/cone_thresh" 2>/dev/null
echo 30 > "$DIR/cone_hold_frames" 2>/dev/null echo 30 > "$DIR/cone_hold_frames" 2>/dev/null
docs/ARCHITECTURE.md
+54 -32
View File
@@ -34,13 +34,12 @@
``` ```
main() main()
├── cfg_load_all() # 从工作目录文件读取全部配置 ├── cfg_load_all() # 从工作目录文件读取全部配置
├── 补读 cone_avoid_gain/range/hold_frames ├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, VL53L0X, 计算巡线尺寸
├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, 计算巡线尺寸
├── ControlInit() # 初始化双电机 GPIO + PWM + 启动编码器线程 ├── ControlInit() # 初始化双电机 GPIO + PWM + 启动编码器线程
│ │
└── while(running): └── while(running):
├── CameraHandler() # ★ 逐帧执行 ├── CameraHandler() # ★ 逐帧执行
└── target_speed = g_cfg.speed # (debug 模式下每 7 帧重载) └── target_speed = g_cfg.speed # (debug 模式下每 30 帧重载)
``` ```
`CameraInit` 现在只接受 `camera_id` 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。 `CameraInit` 现在只接受 `camera_id` 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。
@@ -48,12 +47,12 @@ main()
--- ---
## CameraHandler 11 步流水线 ## CameraHandler 10 步流水线 (`src/camera.cpp:755`)
``` ```
Step 1 capture_frame() 640×480 MJPEG → IMREAD_REDUCED_COLOR_4 Step 1 capture_frame() 640×480 MJPEG → IMREAD_REDUCED_COLOR_4
取帧+解码 → raw_frame = 160×120 BGR 取帧+解码 → raw_frame = 160×120 BGR
(回退: raw_mat 三通道 → 直用 640×480) (回退: raw_mat 三通道 → 直用 640×480)
Step 2 save_image_if_requested() debug=1 && saveImg 文件=1 → 存 ./image/XXXXX.jpg Step 2 save_image_if_requested() debug=1 && saveImg 文件=1 → 存 ./image/XXXXX.jpg
保存图像 (条件) 保存图像 (条件)
@@ -62,36 +61,43 @@ Step 3 image_main() raw_frame → resize(lt_w×lt_h) → HSV 双
视觉巡线 → floodFill(种子 (lt_w/2, lt_h-10), 半径5) 视觉巡线 → floodFill(种子 (lt_w/2, lt_h-10), 半径5)
→ 逐行最长连续段搜索 → 丢线补全(row 10~59) → 逐行最长连续段搜索 → 丢线补全(row 10~59)
输出: left_line[], right_line[], mid_line[] 输出: left_line[], right_line[], mid_line[]
随后: mid_line_raw = mid_line (原始中线快照)
Step 4 run_model_inference() 每 2 帧推理一次 Step 4 lidar_avoid_process() 每 2 帧读距 → 距离→行映射 → 去抖确认
激光避障 确认后中线变形 (固定朝右) + 消失后保持衰减
方向判断基于 mid_line_raw, 变形修改 mid_line
Step 5 run_model_inference() 每 2 帧推理一次
模型推理 raw_frame (160×120) → model_v10_detect() 模型推理 raw_frame (160×120) → model_v10_detect()
4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线 4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
g_thresh = {0.90, 0.75, 0.80, 0.90} g_thresh = {0.65, 0.75, 0.80, 0.82}
→ g_boxes[16], g_box_count → g_boxes[16], g_box_count
Step 5a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s) Step 6a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s)
斑马线状态机 触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee) 斑马线状态机 触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
刹停时 I2C 语音播报 刹停时 I2C 语音播报
Step 5b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN Step 6b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN
红绿灯状态机 红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行 红绿灯状态机 红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行
Step 5c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减 Step 6c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减 + 回弹
锥桶检测 & 中线变形 cone_speed 减速 + cone_hold_frames 保持 锥桶检测 & 中线变形 cone_hold_frames 保持 → cone_return_frames 回弹
Step 6 steering_update() foresee/2 → check_row → mid_line[row]×2 - newWidth/2 Step 7 steering_update() foresee/2 → check_row → mid_line[row]×2 - newWidth/2
舵机控制 deadband 过滤 → servo duty: 1500000 ± offset ns 舵机控制 deadband 过滤 → servo duty: 1500000 ± offset ns
Step 7 motor_update() 开环PWM + 编码器比例刹车 + 弯道减速 + 锥桶减速 Step 8 motor_update() 开环PWM + 编码器比例刹车 + 弯道减速
电机控制 电机控制 (锥桶/挡板减速代码存在但已注释)
Step 8 lcd_render() track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示 Step 9 lcd_render() track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示
LCD 渲染 RGB565 → /dev/fb0 mmap LCD 渲染 RGB565 → /dev/fb0 mmap
Step 9 fps_log() 每 15 帧输出总帧率 + 分步耗时 ms Step 10 fps_log() 分步耗时日志
FPS 统计 FPS 统计
``` ```
另：每 150 帧调用 `malloc_trim(0)` 回收空闲内存（嵌入式优化）。
--- ---
## 视觉巡线深度展开 (`image_main`) ## 视觉巡线深度展开 (`image_main`)
@@ -183,9 +189,10 @@ ControlUpdate(speed, zebra_or_tl_block):
motor[i]->updateduty(0) ← 已静止，不刹车 motor[i]->updateduty(0) ← 已静止，不刹车
return return
curve = 1.0 - |g_steer_deviation| × 0.4 ← 弯道减速 curve = 1.0 - |g_steer_deviation| × g_cfg.curve_slope ← 弯道减速 (默认 0.4)
curve = max(curve, 0.6) ← 最低 ≥ 60% curve = max(curve, g_cfg.curve_min) ← 最低倍率 (默认 0.8)
spd = speed × curve spd = speed × curve
// 锥桶减速 (cone_speed) / 挡板减速 (lidar_speed) 代码存在但已注释
motor[0].updateduty(spd) → 左电机 (pwmchip8/pwm2, gpio12 方向) motor[0].updateduty(spd) → 左电机 (pwmchip8/pwm2, gpio12 方向)
motor[1].updateduty(spd) → 右电机 (pwmchip8/pwm1, gpio13 方向) motor[1].updateduty(spd) → 右电机 (pwmchip8/pwm1, gpio13 方向)
@@ -196,8 +203,8 @@ ControlUpdate(speed, zebra_or_tl_block):
刹车力度与当前车速成正比（`brake_scale`），有上限（`brake_max` ns）。 刹车力度与当前车速成正比（`brake_scale`），有上限（`brake_max` ns）。
速度低于 0.5 pps 时不再施加刹车。 速度低于 0.5 pps 时不再施加刹车。
**motor_update 额外逻辑：** `motor_update()` 在调用 `ControlUpdate` 之前，若锥桶已触发（`cone_is_slow()`）， **motor_update 额外逻辑：** `motor_update()` 中包含锥桶和挡板减速代码（`cone_is_slow()` → `× cone_speed`，
会将 speed 乘以 `g_cfg.cone_speed`（默认 0.5），实现锥桶路段减速。 `lidar_is_active()` → `× lidar_speed`），但当前**已注释**，仅弯道减速生效。
**motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。 **motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。
@@ -347,28 +354,43 @@ lcd_render():
|---|---|---|---|---| |---|---|---|---|---|
| `./speed` | double | 60 | 11 | 目标速度 (% 占空比) | | `./speed` | double | 60 | 11 | 目标速度 (% 占空比) |
| `./start` | int(0/1) | 0 | 0 | 使能开关，1=电机运行 | | `./start` | int(0/1) | 0 | 0 | 使能开关，1=电机运行 |
| `./debug` | int(0/1) | 0 | (不写入) | 每7帧重载配置+允许存图 | | `./debug` | int(0/1) | 0 | (不写入) | 每30帧重载配置+允许存图 |
| `./showImg` | int(0/1) | 0 | (不写入) | LCD 显示（每10帧轮询→g_lcd_on） | | `./showImg` | int(0/1) | 0 | (不写入) | LCD 显示（每10帧轮询→g_lcd_on） |
| `./foresee` | double | 80 | 40 | 舵机前瞻行 (显示空间像素) | | `./foresee` | double | 80 | 40 | 舵机前瞻行 (显示空间像素) |
| `./deadband` | double | 5 | 8 | 舵机死区 (显示空间像素) | | `./deadband` | double | 5 | 8 | 舵机死区 (显示空间像素) |
| `./steer_gain` | double | 1.0 | 1.5 | 舵机增益 | | `./steer_gain` | double | 1.0 | 1.5 | 舵机增益 |
| `./center_bias` | double | 0 | (不写入) | 中线偏置修正 | | `./center_bias` | double | 0 | (不写入) | 中线偏置修正 |
| `./kp`,`./ki`,`./kd` | double | - | 3.5/0.3/2.0 | PID 参数 (当前未使用) |
| `./mortor_kp/ki/kd` | double | - | 0.6/0.2/0 | 电机编码器 PID (当前未使用) |
| `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) | | `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) |
| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) | | `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) | | `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
| `./cone_avoid_gain` | double | 0.3 | 0.3 | 锥桶中线变形推离量 (归一化) | | `./cone_avoid_gain` | double | 0.3 | 0.3 | 锥桶中线变形推离量 (归一化) |
| `./cone_avoid_range` | int | 30 | 30 | 变形斜坡陡峭度 | | `./cone_avoid_range` | int | 30 | 30 | 变形斜坡陡峭度 |
| `./cone_speed` | double | 0.5 | 0.5 | 锥桶触发时速度倍率 | | `./cone_speed` | double | 0.5 | 0.5 | 锥桶触发时速度倍率 (当前未生效) |
| `./cone_min_frames` | int | 3 | 3 | 锥桶连续确认帧数 | | `./cone_min_frames` | int | 1 | 1 | 锥桶确认帧数 (单帧即确认) |
| `./cone_margin` | int | 0 | 0 | 0=中心点, 1=框边缘检查 | | `./cone_margin` | int | 0 | 0 | 0=中心点, 1=框边缘检查 |
| `./cone_thresh` | double | 0.80 | 0.80 | 锥桶置信度阈值 | | `./cone_thresh` | double | 0.80 | 0.80 | 锥桶置信度阈值 |
| `./cone_hold_frames` | int | 30 | 30 | 锥桶消失后保持变形帧数 | | `./cone_hold_frames` | int | 15 | 30 | 锥桶消失后保持变形帧数 |
| `./cone_return_gain` | double | 1.0 | (不写入) | 回弹力度 (1.0=等同避让) |
| `./cone_return_frames` | int | 30 | (不写入) | 回弹持续帧数 |
| `./curve_slope` | double | 0.4 | (不写入) | 弯道减速斜率 |
| `./curve_min` | double | 0.8 | (不写入) | 弯道最低速度倍率 |
| `./brake_scale` | double | 10 | 10 | 刹车速度→占空比系数 | | `./brake_scale` | double | 10 | 10 | 刹车速度→占空比系数 |
| `./brake_max` | int | 10000 | 10000 | 最大刹车占空比 (ns) | | `./brake_max` | int | 10000 | 10000 | 最大刹车占空比 (ns) |
| `./lidar_enable` | int(0/1) | 1 | 1 | 激光避障总开关 |
| `./lidar_thresh` | int | 300 | 300 | 障碍判定距离阈值 (mm) |
| `./lidar_pre` | int | 800 | (不写入) | 预触发去抖窗口 (mm) |
| `./lidar_near` | int | 50 | 50 | row=lt_h-1 对应距离 (mm) |
| `./lidar_far` | int | 1200 | 1200 | row=10 对应距离 (mm) |
| `./lidar_near_start` | int | 1 | 1 | near_valid 起始偏移 |
| `./lidar_near_end` | int | 4 | 4 | near_valid 终止偏移 |
| `./lidar_far_span` | int | 6 | 6 | far_lost 检测跨度 |
| `./lidar_min_frames` | int | 3 | 3 | 连续确认帧数 |
| `./lidar_avoid_gain` | double | 0.4 | 0.4 | 绕行推离力度 (归一化) |
| `./lidar_avoid_range` | int | 30 | 30 | 绕行斜坡陡峭度 |
| `./lidar_speed` | double | 0.4 | 0.4 | 绕行速度倍率 (当前未生效) |
| `./lidar_hold_frames` | int | 30 | 30 | 消失后保持帧数 |
**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。 **debug 模式：** `./debug` = 1 时，主循环每 30 帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
**注意：** `global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。 **注意：** `global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
@@ -380,12 +402,12 @@ lcd_render():
| 文件 | 功能 | 排除原因 | | 文件 | 功能 | 排除原因 |
|---|---|---| |---|---|---|
| `vl53l0x.cpp` | 激光测距模块 | 硬件未连接 |
| `zebra_detect.cpp` | 经典斑马线检测 | 已被 Mild 模型替代 | | `zebra_detect.cpp` | 经典斑马线检测 | 已被 Mild 模型替代 |
| `PIDController.cpp` | 位置式/增量式 PID | 电机开环直驱，无调用点 | | `PIDController.cpp` | 位置式/增量式 PID | 电机开环直驱，无调用点 |
| `serial.cpp` | VOFA 串口可视化 | 未使用 | | `serial.cpp` | VOFA 串口可视化 | 未使用 |
**注意：** `PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比）， **注意：** `vl53l0x.cpp` 已恢复编译（激光测距已集成至 `lidar_avoid_process()`）。
`PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比），
不提供 `updateSpeed()` 方法。 不提供 `updateSpeed()` 方法。
--- ---
@@ -396,7 +418,7 @@ lcd_render():
- **输入**：160×120 BGR - **输入**：160×120 BGR
- **输出**：4 类 + 背景 → 9 通道：0=锥桶, 1=红灯, 2=绿灯, 3=斑马线 - **输出**：4 类 + 背景 → 9 通道：0=锥桶, 1=红灯, 2=绿灯, 3=斑马线
- **架构**：3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9) - **架构**：3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9)
- **阈值**：`{0.90, 0.75, 0.80, 0.90}`（锥桶/红灯/绿灯/斑马线） - **阈值**：`{0.65, 0.75, 0.80, 0.82}`（锥桶/红灯/绿灯/斑马线）
- **权重文件**：`mild_v12.bin`（105KB 自定义二进制格式） - **权重文件**：`mild_v12.bin`（105KB 自定义二进制格式）
- **推理频率**：每 2 帧一次（跳帧节省 CPU） - **推理频率**：每 2 帧一次（跳帧节省 CPU）
docs/CONE_DESIGN.md
+170 -80
View File
@@ -4,7 +4,27 @@
**不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。 **不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。
不使用任何推入计时器、归还逻辑、EMA 平滑——锥桶消失后，下一帧 `image_main()` 重新计算 `mid_line`，自动恢复。 锥桶消失后进入**保持衰减**阶段（`cone_hold_frames` 帧内变形量线性衰减），随后进入**回弹**阶段（`cone_return_frames` 帧内朝锥桶方向反推，帮助车回到赛道中央）。
### 1.1 原始中线快照 (`mid_line_raw`)
避障变形会原地修改 `mid_line[]`，导致方向判断被污染。为解决此问题，在 `image_main()` 之后立即保存一份快照：
```
image_main() → mid_line[] = 赛道真实中线
mid_line_raw = mid_line → 快照 (60 个 int, 240 字节, 每帧复制)
lidar_avoid_process() → 修改 mid_line[] (lidar 推力)
cone_detect_and_deform() → 修改 mid_line[] (锥桶推力)
steering_update() → 读 mid_line[] (变形后) 控制舵机
```
| 变量 | 含义 | 来源 | 被修改 | 用途 |
|------|------|------|--------|------|
| `mid_line_raw[]` | 赛道在哪 | `image_main()` 每帧重算 | 否 | 方向判断 |
| `mid_line[]` | 车该往哪走 | `mid_line_raw` + 避障推力 | 是 | 舵机控制 |
**所有方向判断（锥桶在赛道左侧还是右侧、回弹方向）均基于 `mid_line_raw`**，
避免变形后的中线导致方向翻转。
--- ---
@@ -158,126 +178,196 @@ foresee 行 (row=20) 收到 33% 的偏移，产生 +8px 偏差 → 刚好越过
## 6. 去抖机制 ## 6. 去抖机制
与之前一致，不改变：
``` ```
#define CONE_MIN_FRAMES 3
#define CONE_COUNT_DECAY 1
g_cone_frames: 连续检测计数 g_cone_frames: 连续检测计数
g_cone_last_row / g_cone_last_col: 位置跟踪 (容差 ±5行/±10列)
g_cone_confirmed = (g_cone_frames >= CONE_MIN_FRAMES) 位置容忍 (动态):
row_tol = max(2, line_tracking_height / 12) 典型值 5
col_tol = max(3, line_tracking_width / 8) 典型值 10
首次出现: g_cone_frames = 1, 记录位置
位置接近 (容差内): g_cone_frames++, 更新位置
位置突变: g_cone_frames = 1, 更新位置 (重新开始计数)
未检测到: g_cone_frames = max(0, g_cone_frames - 1)
g_cone_confirmed = (g_cone_frames >= cone_min_frames)
``` ```
**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。 **cone_min_frames 默认值为 1（单帧即确认）。** 原因：视觉巡线（`image_main()` 的 floodFill）
- 锥桶在 → 变形在（每帧重新计算 mid_line 变形） 对赛道上的锥桶响应极快——锥桶进入视野后 1~2 帧内车就开始转弯，锥桶随即离开视野。
- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失 模型检测的窗口天然只有 1~2 帧，多帧去抖门槛（如 3）永远达不到。
- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑 边界过滤（row 有效、赛道内、conf ≥ cone_thresh）已经提供了足够的误触发保护。
--- ---
## 7. 舵机与电机 ## 7. 保持衰减 + 回弹
**舵机：** 零改动。Step 6 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。 锥桶消失后不会立刻恢复正常巡线，而是经历三个阶段：
**电机：** 推入期间减速：
``` ```
if (g_cone_confirmed && g_zstate != Z_STOP) { 确认 (g_cone_confirmed && cone_seen)
effective_speed = target_speed * cone_speed_factor; → g_cone_hold_ctr = 1, g_cone_return_ctr = 0
} else { → 冻结位置: hold_src_row/col = cone_row/col
effective_speed = target_speed; → 记录回弹方向: 锥桶在中线左侧 → return_dir = -1 (回弹推左)
锥桶在中线右侧 → return_dir = +1 (回弹推右)
确认但不可见 (g_cone_confirmed && !cone_seen, 去抖衰减中)
→ g_cone_hold_ctr = 1 (保持重置, 但不覆盖冻结位置)
→ 使用 hold_src_row/col 继续避让变形
保持衰减 (!g_cone_confirmed, 1 ~ cone_hold_frames 帧)
→ g_cone_hold_ctr++
→ decay = 1.0 - hold_ctr / hold_frames (线性衰减到 0)
→ 中线变形量乘 decay → 逐渐恢复
回弹 (保持结束后 1 ~ cone_return_frames 帧)
→ g_cone_return_ctr++
→ 反方向推中线 (return_dir), 力度 = cone_return_gain
→ 斜坡范围 = cone_avoid_range × 1.2 (比避让略缓)
→ 覆盖全部有效行 (row 10 ~ lt_h-1), 不限于锥桶位置
→ decay = 1.0 - return_ctr / return_frames (线性衰减)
→ 回弹结束: 清零所有状态变量
```
**cone_is_slow() 在三个阶段中均返回 true**（确认 / 保持 / 回弹），但锥桶减速代码当前**已注释**。
### 7.1 已修复的 BUG：hold_src_row 被 -1 覆盖导致回弹永远不执行
**问题**：锥桶消失后，由于去抖计数 `g_cone_frames` 逐帧 -1 衰减，`g_cone_confirmed`
在锥桶消失后仍保持 true 多帧（如锥桶可见 20 帧 → 消失后 confirmed 维持 18 帧）。
在这些帧中 `cone_detect_and_deform()` 内的局部变量 `cone_row = -1`（没找到锥桶），
但旧代码在 `if (g_cone_confirmed)` 块中无条件执行：
```cpp
// ★ 旧代码 (BUG):
if (g_cone_confirmed) {
g_cone_hold_ctr = 1;
g_cone_hold_src_row = cone_row; // cone_row = -1, 覆盖有效值!
g_cone_hold_src_col = cone_col; // 同上
g_cone_return_ctr = 0;
double ms = mid_line[cone_row]; // mid_line[-1] 越界访问!
g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0;
} }
ControlUpdate(effective_speed, g_zstate == Z_STOP);
``` ```
后果链：
1. `hold_src_row` 被 -1 覆盖 → 有效位置丢失
2. `mid_line[-1]` 越界访问 → 未定义行为
3. 后续所有阶段 `src_row = hold_src_row = -1` → `if (src_row <= 10) return` 提前退出
4. 保持变形 **从不执行**，回弹变形 **从不执行**
```
时间轴: 锥桶可见 20帧 去抖衰减 18帧 hold 15帧 return 30帧
避让变形: ████████████████████ (src_row=-1,退出) (同上,-1) (同上,-1)
保持变形: 从不执行 从不执行
回弹变形: 从不执行
```
**修复**：只在 `cone_seen` 为 true 时更新冻结位置和回弹方向：
```cpp
// ★ 修复后:
if (g_cone_confirmed) {
g_cone_hold_ctr = 1;
g_cone_return_ctr = 0;
if (cone_seen) { // 只在锥桶实际可见时更新
g_cone_hold_src_row = cone_row;
g_cone_hold_src_col = cone_col;
double ms = mid_line[cone_row];
g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0;
}
}
```
同时修复 `src_row` 选择逻辑：
```cpp
// ★ 旧代码:
int src_row = g_cone_confirmed ? cone_row : g_cone_hold_src_row;
// 当 confirmed=true 但 cone_seen=false 时 → src_row = cone_row = -1 → 提前退出
// ★ 修复后:
int src_row = (g_cone_confirmed && cone_seen) ? cone_row : g_cone_hold_src_row;
// confirmed 但不可见时 → 使用冻结位置, 避让变形继续生效
```
修复后的时间轴：
```
时间轴: 锥桶可见 20帧 去抖衰减 18帧 hold 15帧 return 30帧
避让变形: ████████████████████ ████████████████████ ▓▓▓▓▓▓▓▓▓▓▓▓▓▓
保持变形: (用冻结位置继续避让) (线性衰减→0)
回弹变形: ░░░░░░░░░░░░░░
```
## 8. 舵机与电机
**舵机：** 零改动。steering_update 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。
**电机：** `motor_update()` 中存在锥桶减速代码（`cone_is_slow()` → `speed × cone_speed`），
但当前**已注释**，仅弯道减速生效。如需启用，取消 `motor_update()` 中对应行的注释即可。
--- ---
## 8. 参数配置 ## 9. 参数配置
### 8.1 启动时一次性读取（运行中不重新读文件） 所有锥桶参数均通过 `cfg_load_all()` 统一读取（`src/global.cpp`），debug 模式下每 30 帧热更新。
| 文件 | 类型 | 默认 | 含义 | | 文件 | 类型 | 默认 | 含义 |
|---|---|---|---| |---|---|---|---|
| `./cone_avoid_gain` | double | 0.3 | 归一化推离量 (0=不推, 1=推到赛道边缘) | | `./cone_avoid_gain` | double | 0.3 | 归一化推离量 (0=不推, 1=推到赛道边缘) |
| `./cone_avoid_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) | | `./cone_avoid_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) |
| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 (当前减速代码已注释) |
### 8.2 热更新参数 | `./cone_min_frames` | int | 1 | 确认帧数 (1=单帧即确认) |
| 文件 | 类型 | 默认 | 含义 |
|---|---|---|---|
| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 |
| `./cone_min_frames` | int | 3 | 连续确认帧数 |
| `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 | | `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 |
| `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) | | `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) |
| `./cone_hold_frames` | int | 15 | 消失后保持变形帧数 |
### 8.3 读取实现 | `./cone_return_gain` | double | 1.0 | 回弹力度 (1.0=等同避让力度) |
| `./cone_return_frames` | int | 30 | 回弹持续帧数 |
**main.cpp 启动时（一次性）：**
```cpp
double avoid_gain_val = readDoubleFromFile("./cone_avoid_gain");
int avoid_range_val = (int)readDoubleFromFile("./cone_avoid_range");
if (avoid_gain_val <= 0) avoid_gain_val = 0.3;
if (avoid_range_val <= 0) avoid_range_val = 30;
g_cfg.cone_avoid_gain = avoid_gain_val;
g_cfg.cone_avoid_range = avoid_range_val;
```
**global.cpp (cfg_load_all，热更新)：**
```cpp
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file);
```
**ctl.sh：**
```sh
echo 0.3 > "$DIR/cone_avoid_gain" 2>/dev/null
echo 30 > "$DIR/cone_avoid_range" 2>/dev/null
echo 0.5 > "$DIR/cone_speed" 2>/dev/null
echo 3 > "$DIR/cone_min_frames" 2>/dev/null
echo 0 > "$DIR/cone_margin" 2>/dev/null
echo 0.80 > "$DIR/cone_thresh" 2>/dev/null
```
--- ---
## 9. 代码修改清单 ## 10. 代码位置
``` ```
lib/image_cv.h:
└── extern mid_line_raw (原始中线快照声明)
src/image_cv.cpp:
└── std::vector<int> mid_line_raw (定义)
lib/global.h: lib/global.h:
├── CfgCache 新增: cone_avoid_gain, cone_avoid_range, └── CfgCache 包含: cone_avoid_gain, cone_avoid_range, cone_speed,
│ cone_speed, cone_min_frames, cone_margin, cone_thresh cone_min_frames, cone_margin, cone_thresh,
└── 新增文件名常量 cone_hold_frames, cone_return_gain, cone_return_frames
src/global.cpp: src/global.cpp:
└── cfg_load_all() 追加热更新参数 (不含 avoid_gain/avoid_range) └── cfg_load_all() 统一读取所有锥桶参数
main/main.cpp:
└── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg
src/camera.cpp: src/camera.cpp:
├── #define CONE_CLASS 0 ├── CameraHandler(): image_main() 后执行 mid_line_raw = mid_line (快照)
├── 新增静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed ├── 静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed,
├── Step 5 之后: 插入锥桶检测+边界过滤+去抖 │ g_cone_hold_ctr, g_cone_hold_src_row/col,
├── Step 5.5: 中线变形 (修改 mid_line[row] for row in [10, cone_row]) │ g_cone_return_ctr, g_cone_return_dir
├── Step 7 电机: g_cone_confirmed → effective_speed *= cone_speed_factor ├── cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段
└── Step 8 LCD: 锥桶框改橙色 ├── cone_detect_and_deform(): 方向判断基于 mid_line_raw, 变形修改 mid_line
├── motor_update(): cone_speed 减速 (已注释)
└── lcd_render(): 锥桶框橙色绘制
ctl.sh: ctl.sh:
└── init_pins() 追加 6 个新文件默认值 └── init_pins() 写入 cone_avoid_gain/range/speed/min_frames/margin/thresh/hold_frames
``` ```
--- ---
## 10. 方案对比 ## 11. 方案对比
| | 旧方案（舵机推入） | 新方案（中线变形） | | | 旧方案（舵机推入） | 当前方案（中线变形） |
|---|---|---| |---|---|---|
| 舵机代码 | 需改动 | 零改动 | | 舵机代码 | 需改动 | 零改动 |
| deadband | 需绕过 | 自 然生效 | | deadband | 需绕过 | 自然生效 |
| steer_gain | 需单独 cone_gain | 共用 | | steer_gain | 需单独 cone_gain | 共用 |
| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) | | 归还机制 | 需计时器+衰减 | hold 衰减 + return 回弹 |
| 平滑性 | 需 EMA | 斜坡自带平滑 | | 平滑性 | 需 EMA | 斜坡自带平滑 |
| 参数数量 | 7 | 6 | | 参数数量 | 7 | 9 (含 hold/return) |
| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 | | 锥桶消失恢复 | 需手动衰减 | 自动: hold 衰减 → return 回弹 → 正常 |
docs/ENCODER_BRAKE.md
+84 -57
View File
@@ -2,7 +2,7 @@
## 1. 硬件 ## 1. 硬件
编码器2，引脚： 编码器引脚：
| 信号 | 引脚 | 模式 | | 信号 | 引脚 | 模式 |
|---|---|---| |---|---|---|
@@ -11,77 +11,99 @@
两引脚均属 gpiochip64（GPA64，base=64，16 pins）。 两引脚均属 gpiochip64（GPA64，base=64，16 pins）。
## 2. 触发条件 ## 2. 编码器线程
`ControlUpdate(speed, zebra_block)` 中 `zebra_block == true` 时进入刹车流程。`zebra_block` 由 `CameraHandler` Step 5 斑马线状态机传入——当 `g_zstate == Z_STOP` 时为 true。 `ControlInit()` 启动独立线程 `encoder_thread()`（`src/control.cpp:30`），持续运行直到 `ControlExit()`。
## 3. 刹车状态机
``` ```
┌──────────┐ encoder_thread():
正常行驶 ───→ │ IDLE │ 高频轮询 GPIO67 (LSB 边沿) + GPIO72 (方向)
└────┬─────┘ 轮询间隔: 500µs sleep (~2kHz)
│ zebra_block==true
▼ 每 100ms 窗口:
┌──────────┐ g_enc_speed = pulse_cnt / elapsed_time (脉冲/秒, 原子变量)
│ BRAKING │ ← 施加 -30% 占空比 g_enc_dir = GPIO72 当前值 (原子变量)
│ │ 每帧读 GPIO 67 pulse_cnt 归零, 开始新窗口
└────┬─────┘
┌───────────┴───────────┐
│ 脉冲变化 │ 脉冲不变
▼ ▼
轮子在转 → 继续刹车 g_brake_still++
重置计数器 │
连续 5 帧无变化？
或超时 60 帧？
│
▼
┌──────────┐
│ HOLDING │ ← duty=0，保持静止
└──────────┘
│
zebra_block==false
│
▼
┌──────────┐
│ IDLE │
└──────────┘
``` ```
## 4. 参数 通过 sysfs 文件描述符轮询（`GPIO::readValue()`），`lseek` + `read`。不依赖中断。
定义在 `src/control.cpp`： ## 3. 触发条件
| 常量 | 值 | 含义 | `ControlUpdate(speed, block)` 中 `block == true` 时进入刹车流程。
|---|---|---|
| `BRAKE_STILL_THRESH` | 5 | 连续无脉冲帧数阈值，超过即判停 |
| `BRAKE_TIMEOUT` | 60 | 最长刹车帧数（~2s @30fps），强制释放 |
| `BRAKE_DUTY` | -30 | 刹车占空比（-100~100，负值=反转） |
## 5. 读取方式 `block` 信号来源（两者取 OR）：
- **斑马线**: `zebra_process()` 返回 true 当 `g_zstate == Z_STOP`
- **红绿灯**: `traffic_light_process()` 返回 true 当 `g_tl_state != TL_NORMAL`
GPIO 67 通过 sysfs 文件描述符轮询（`GPIO::readValue()`），每帧 `lseek` + `read` 一次。不依赖中断。 调用链: `motor_update(zebra_block, tl_block)` → `ControlUpdate(final_spd, zebra_block || tl_block)`
**为什么轮询够用：** 30fps 帧率 = 33ms 间隔。编码器假设 100+ 脉冲/转，即使 1 转/秒的极慢速度也有 3+ 脉冲/帧间隔，不会漏判。 ## 4. 比例刹车
## 6. 刹车强度 无状态机，每帧根据编码器实时速度计算刹车力度：
``` ```
motor[i]->updateduty(-30) ControlUpdate(speed, zebra_block):
if !g_cfg.start → 两电机 duty=0, GPIO73 拉低, return
if zebra_block:
cur_speed = g_enc_speed.load() ← 编码器线程原子读
cur_dir = g_enc_dir.load()
if cur_speed > 0.5: ← 仍在运动
brake_ns = cur_speed × g_cfg.brake_scale
brake_ns = clamp(brake_ns, 0, g_cfg.brake_max)
brake_pct = brake_ns / 500.0 ← ns → % (period=50000ns)
motor[i]->updateduty(±brake_pct) ← 反向占空比 (方向取反)
else: ← 已停止 (< 0.5 pps)
motor[i]->updateduty(0) ← 不施加刹车
return
正常行驶:
motor[0].updateduty(speed)
motor[1].updateduty(speed)
mortorEN.setValue(1)
``` ```
`MotorController::updateduty()` 内： **特点：**
- 占空比：`period * |duty| / 100 = 50000 * 30 / 100 = 15000 ns` - **比例刹车**: 速度越快 → 刹车占空比越大 → 刹车力越强
- 方向：`duty < 0` → `directionGPIO.setValue(false)` → 反转 - **自动停止**: 速度降到 0.5 pps 以下自动释放刹车，防止过冲
- **反向驱动**: 根据编码器方向 `cur_dir` 施加反方向 PWM
即 30% 占空比的反向驱动，足够刹停但不会让车向后猛冲。 ## 5. 参数
## 7. 编码器生效范围 可通过文本文件热更新（debug 模式下每 30 帧重载）：
- **仅在斑马线 STOP 时**使用编码器 | 文件 | 类型 | 默认 | 含义 |
- 正常行驶时编码器处于"打开但未使用"状态（fd 保持 open，但不 read） |---|---|---|---|
- 锥桶避障不使用编码器 | `./brake_scale` | double | 10 | 速度(pps) → 刹车占空比(ns) 缩放系数 |
- `g_cfg.start == 0` 时刹车状态机重置 | `./brake_max` | int | 10000 | 最大刹车占空比 (ns), 上限保护 |
**数值示例：** 编码器读数 500 pps → brake_ns = 500 × 10 = 5000 ns → brake_pct = 5000/500 = 10%
（period=50000ns, 即 duty = 50000 × 10/100 = 5000 ns 的 PWM 反转输出）
## 6. `updateduty()` 内部
```
MotorController::updateduty(duty):
pwm_ns = period × |duty| / 100 ← 转为占空比 ns
→ 写入 sysfs duty_cycle
duty > 0 → GPIO 方向 = 1 (正转)
duty ≤ 0 → GPIO 方向 = 0 (反转)
```
motor[0]: pwmchip8/pwm2 + GPIO12（左电机）
motor[1]: pwmchip8/pwm1 + GPIO13（右电机）
GPIO73: 电机使能 (mortorEN)
## 7. 生效范围
- **斑马线 STOP + 红绿灯 STOP/WAIT_GREEN** 时使用编码器刹车
- 正常行驶时编码器线程持续运行但刹车不触发
- 锥桶/挡板避障不使用编码器刹车（仅减速+绕行）
- `g_cfg.start == 0` 时全部电机停转，不进入刹车流程
## 8. 关键代码路径 ## 8. 关键代码路径
@@ -89,7 +111,12 @@ motor[i]->updateduty(-30)
main.cpp main.cpp
└── while(running) └── while(running)
└── CameraHandler() └── CameraHandler()
└── ControlUpdate(target_speed, g_zstate == Z_STOP) ├── zebra_process() → zebra_block
└── if (zebra_block) → 刹车状态机 ├── traffic_light_process() → tl_block
└── encoderLSB.readValue() ← GPIO 67 └── motor_update(zebra_block, tl_block)
└── ControlUpdate(final_spd, zebra_block || tl_block)
└── if (block) → 读 g_enc_speed/g_enc_dir → 比例刹车
ControlInit()
└── encoder_thread() 后台运行 → g_enc_speed, g_enc_dir
``` ```
docs/LIDAR_AVOID.md
+99 -255
View File
@@ -1,232 +1,130 @@
# 激光雷达挡板避障设计 # 激光雷达挡板避障设计
## 问题 ## 概述
VL53L0X 是单点 ToF 激光测距传感器，仅返回沿光束方向的一个距离值（mm），没有扇形扫描能力。 VL53L0X 单点 ToF 激光测距传感器，返回沿光束方向的距离值（mm）。
当激光测到近处有物体时，无法直接从距离值判断它是： 当距离低于阈值时触发绕行：中线变形推向右侧，消失后保持衰减。
- A) **挡板/障碍物** — 横在赛道上的物体，需要绕行 **当前实现：** 纯距离触发 + 预触发去抖，不检查视觉边界线。绕行方向固定朝右。
- B) **赛道边墙** — 弯道处车头对准了侧墙，这是正常行驶状态
**解决思路：用视觉巡线的边界线（left_line / right_line）来区分 A 和 B。**
--- ---
## 关键约束：挡板会导致丢线 ## VL53L0X 驱动集成
挡板立在赛道上时，不仅触发激光近距读数，还会**遮挡摄像头视野中的赛道边界**， 驱动已编译（`src/vl53l0x.cpp` + `lib/vl53l0x/` C API），在 `CameraInit()` 中初始化：
导致从挡板所在位置开始边界线丢失（left_line / right_line = -1）。
``` ```
Camera 俯视视角 (图像坐标): CameraInit():
row 0 (远) : ░░░░░░░ ← 被挡板挡住，丢线 if g_cfg.lidar_enable:
row 15 : ░░░░░░░ ← 挡板上方，丢线 g_lidar_ok = g_lidar_sensor.init() // open /dev/stmvl53l0x_ranging
row 25 : ░░░░░░░ ← 挡板顶部附近，丢线 g_lidar_sensor.startMeasure() // 启动首次测量
row 30 : ███████ ← ★ 挡板所在行，边界线丢失 else:
row 35 : ■■■■■■■ ← 挡板下方，赛道可见，边界线有效 g_lidar_ok = false // 禁用
row 59 (近) : ■■■■■■■ ← 车前方，赛道清晰
cameraDeInit():
if g_lidar_ok → g_lidar_sensor.stop()
``` ```
**这意味着：不能像锥桶检测那样"往更远处看边线是否还开着"，因为挡板后面的边线必然丢失。** 硬件未连接时 `init()` 失败 → `g_lidar_ok = false` → `lidar_avoid_process()` 直接跳过，不影响正常行驶。
正确的判定是检测 **"有效边界 → 丢线"的过渡位置是否与激光近距读数对应**：
- 紧贴着挡板下方（更近处）：赛道应该可见，边界有效
- 挡板位置及上方（更远处）：边界丢失
- 激光读数：短距离 → 同一位置有物理障碍物
三个条件同时成立 → 挡板确认。
--- ---
## 几何模型 ## 流水线位置
`lidar_avoid_process()` 位于 CameraHandler Step 4，在视觉巡线之后、模型推理之前：
``` ```
Camera + Laser Step 3 image_main() → left_line[], right_line[], mid_line[]
｜ (高度 H, 俯角 θ) Step 4 lidar_avoid_process() → 修改 mid_line[] (绕行变形) ★
｜╲ Step 5 run_model_inference()
｜ ╲ laser beam Step 6c cone_detect_and_deform() → 也修改 mid_line[] (叠加在 lidar 之上)
｜ ╲
ground ──────────────┴────███████──── 挡板 at distance d_mm
(挡板后方赛道被遮挡)
``` ```
- 激光光束沿车体正前方（图像中轴线） lidar 先于 cone 修改 mid_line，优先级更高。cone 的 clamp 确保不超出边界。
- 距离 d_mm 越小 → 物体越近 → 映射到图像中越靠下的行（row 大）
- 距离 d_mm 越大 → 物体越远 → 映射到图像中越靠上的行（row 小）
--- ---
## 核心算法 ## 核心算法
### 第一步：读取激光距离 ### 1. 读取激光距离（每 2 帧一次）
``` ```
d_mm = vl53l0x.readRange().RangeMilliMeter lidar_avoid_process():
if !g_lidar_ok || !g_cfg.lidar_enable → return
if ++lidar_skip < 2 → return // 每 2 帧一读 (~15Hz)
lidar_skip = 0
if d_mm >= LIDAR_THRESHOLD_MM: data = g_lidar_sensor.readResult() // 非阻塞读取上次测量结果
return CLEAR // 远处无障碍，不做任何处理 d_mm = data.RangeMilliMeter
g_lidar_sensor.startMeasure() // 立即启动下次测量 (后台 ~20ms)
``` ```
`LIDAR_THRESHOLD_MM` 是触发阈值。只有距离小于此值才进入判定。建议默认 ~300mm。 ### 2. 预触发去抖
### 第二步：距离 → 图像行映射
``` ```
// 线性模型: if d_mm == 0: // 读失败, 跳过
// row = lt_h-1 (底行, 最近) 对应 D_NEAR skip
// row = 10 (最远有效行) 对应 D_FAR
// clamp 到 [10, lt_h-1]
row = lt_h - 1 - (d_mm - D_NEAR) / (D_FAR - D_NEAR) * (lt_h - 11) if d_mm >= g_cfg.lidar_pre: // 距离超出预触发窗口 (默认 800mm)
row = clamp(row, 10, lt_h - 1) g_lidar_frames = max(0, frames-1) // 衰减但不归零
skip
// d_mm < lidar_pre → 进入触发判定
``` ```
**标定值（需根据实际安装位置测量）：** `lidar_pre`（默认 800mm）是预触发窗口：距离进入此范围就开始攒帧，但要 < `lidar_thresh`（默认 300mm）才真正确认。
| 参数 | 含义 | 建议初值 | ### 3. 距离 → 图像行映射
|------|------|----------|
| `D_NEAR` | row = lt_h-1 对应的物理距离 | 50 mm |
| `D_FAR` | row = 10 对应的物理距离 | 1200 mm |
标定方法：在赛道前方 300mm、600mm、900mm 处各放一个挡板，记录图像中挡板出现的 row，线性拟合。
### 第三步：用边线判定障碍物（核心）
``` ```
// 设 row = distance_to_row(d_mm)，即激光测距对应的图像行 row = lt_h - 1 - (d_mm - lidar_near) × (lt_h - 11) / (lidar_far - lidar_near)
row = clamp(row, 10, lt_h - 1)
// ── 3a. 检查"挡板下方"（更近处）：赛道应该仍可见 ──
near_valid = false
near_ref_row = -1
for r = min(row + NEAR_START, lt_h-1) down to max(row + NEAR_END, lt_h-1):
if left_line[r] != -1 && right_line[r] != -1:
near_valid = true
near_ref_row = r // 记录有效行，后续绕行时判断宽侧
break
// ── 3b. 检查"挡板位置及上方"（更远处）：边界应该已丢失 ──
far_lost = false
for r = row down to max(row - FAR_SPAN, 10):
if left_line[r] == -1 || right_line[r] == -1:
far_lost = true
break
// ── 3c. 联合判定 ──
if near_valid && far_lost:
→ OBSTACLE_CONFIRMED
记录: g_lidar_obstacle_row = row
g_lidar_ref_row = near_ref_row // 用于判断绕行方向
else:
→ CLEAR
``` ```
**判定原理（三种典型场景）：** | 参数 | 默认 | 含义 |
|------|------|------|
| `lidar_near` | 50 mm | row = lt_h-1（最近行）对应的距离 |
| `lidar_far` | 1200 mm | row = 10（最远有效行）对应的距离 |
### 4. 触发判定
``` ```
场景 A: 挡板挡路 → 触发绕行 g_lidar_frames++
d_mm = 300mm → row = 35 g_lidar_obstacle_row = row
row 36~39 (近处): ✓ 赛道可见
row 30~35 (挡板处): ✗ 丢线 (挡板遮挡)
→ near_valid=true, far_lost=true → ★ 触发
场景 B: 正常直道，无障碍 g_lidar_confirmed = (d_mm < lidar_thresh) && (g_lidar_frames >= lidar_min_frames)
d_mm = 1200mm → row = 10, 且 d_mm > THRESHOLD
→ 不进入判定，直接 CLEAR
场景 C: 弯道，激光打到边墙
d_mm = 200mm → row = 40
row 41~44 (近处): ✓ 赛道可见
row 37~40 (弯道处): ✓ 赛道也可能可见（边墙不一定导致丢线）
→ near_valid=true, far_lost=false → CLEAR
``` ```
### 第四步：去抖确认 **注意：** 不检查视觉边界线（near_valid / far_lost），纯距离触发。
配置中的 `lidar_near_start`、`lidar_near_end`、`lidar_far_span` 参数被加载但**未使用**（为设计预留）。
### 5. 确认 → 记录状态
``` ```
if OBSTACLE_CONFIRMED: if g_lidar_confirmed:
g_lidar_frames++ g_lidar_hold_ctr = 0
else: g_lidar_hold_src_row = g_lidar_obstacle_row
g_lidar_frames = max(0, g_lidar_frames - 1) g_lidar_hold_dir = 1.0 // 固定朝右绕行
g_lidar_frames = 0 // 重置计数
g_lidar_confirmed = (g_lidar_frames >= LIDAR_MIN_FRAMES)
``` ```
**方向固定为右（+1.0）**，不根据赛道左右空间动态判断。
--- ---
## 绕行策略：中线变形 ## 中线变形
挡板通常只挡赛道的一部分（偏左或偏右），通过判断挡板下方有效行中哪一侧空间更大， 当 `lidar_is_active()`（确认中或保持衰减中）时，对 row 10 ~ 障碍行施加变形：
将中线推向宽侧实现绕行。
### 判断绕行方向
``` ```
// 在 near_ref_row（挡板下方最近的有效行）中判断左右空间 decay = (确认中) 1.0 : (1.0 - hold_ctr / hold_frames) // 保持期线性衰减
left_space = mid_line[near_ref_row] - left_line[near_ref_row]
right_space = right_line[near_ref_row] - mid_line[near_ref_row]
// 往宽侧推 for row = 10 to g_lidar_hold_src_row:
dir = (left_space > right_space) ? +1.0 : -1.0 t = clamp((row - 10) / lidar_avoid_range, 0.0, 1.0)
``` push = t × lidar_avoid_gain × half_w × dir × decay
mid_line[row] = clamp(mid_line[row] + push,
### 中线变形 left_line[row] + 2.0,
right_line[row] - 2.0)
对从 row=10 到挡板位置 row 的所有行施加变形，变形量从远到近线性斜坡上升：
```
// gain = lidar_avoid_gain (推离力度, 归一化)
// range = lidar_avoid_range (斜坡陡峭度, 行数)
// half_w = lt_w / 2
for r = 10 to g_lidar_obstacle_row:
t = clamp((r - 10) / range, 0.0, 1.0) // 斜率上升
push = t * gain * half_w * dir
mid_line[r] = clamp(mid_line[r] + push,
left_line[r] + 2.0,
right_line[r] - 2.0)
```
**变形示意图：**
```
row 10 ───────●──────────────────────────○ 赛道中心线
row 20 ────────●─────────────────────────○ (往右绕行)
row 30 ───────────●──────────────────────○
row 35 ──────────────●───────────────────○ ← 挡板位置
row 40 ────────────────████████████──────── ← 挡板 (丢线)
row 59 ─■────────●───■■■■■■■■■■■■■■───●───■ ← 车底 (近处可见)
left mid 方向盘往右打 right
```
### 速度联动
绕行时降速以保证安全：
```
if g_lidar_confirmed || g_lidar_hold_ctr > 0:
spd *= lidar_speed // 默认 0.4，即降至 40% 速度
```
### 消失后保持衰减
挡板不再被检测到后，变形不会立即消失，而是线性衰减（类似锥桶 hold 逻辑）：
```
// 确认状态
if g_lidar_confirmed:
g_lidar_hold_ctr = 0
g_lidar_hold_src_row = g_lidar_obstacle_row
g_lidar_hold_dir = dir
// 衰减状态（挡板消失后）
else if g_lidar_hold_src_row > 0 && g_lidar_hold_ctr < lidar_hold_frames:
g_lidar_hold_ctr++
decay = 1.0 - (double)g_lidar_hold_ctr / lidar_hold_frames // 线性衰减到 0
for r = 10 to g_lidar_hold_src_row:
t = clamp((r - 10) / range, 0.0, 1.0)
push = t * gain * half_w * g_lidar_hold_dir * decay
mid_line[r] = clamp(mid_line[r] + push, left+2, right-2)
``` ```
--- ---
@@ -237,107 +135,53 @@ else if g_lidar_hold_src_row > 0 && g_lidar_hold_ctr < lidar_hold_frames:
┌────────────────────────┐ ┌────────────────────────┐
│ │ │ │
▼ │ ▼ │
┌──────┐ 确认 ┌──────┐ 消失 ┌──────────┐ 保持结束 ┌──────┐ 确认 ┌──────┐ 消失 ┌──────────┐ 保持结束
│NORMAL│ ─────► │AVOID │ ─────► │HOLD_DECAY│ ────────► NORMAL │NORMAL│ ─────► │AVOID │ ─────► │HOLD_DECAY│ ────────► NORMAL
│ │ │ │ │ (衰减) │ │ │ │ │ │ (衰减) │
└──────┘ └──────┘ └──────────┘ └──────┘ └──────┘ └──────────┘
▲ │ ▲ │
└────────────────────────────────┘ 测距恢复正常 └────────────────────────────────┘ 测距恢复正常
NORMAL: 正常行驶，激光测距 > THRESHOLD NORMAL: 正常行驶，激光测距 > lidar_pre
AVOID: 挡板确认 → 中线变形绕行 + 减速 × lidar_speed AVOID: 挡板确认 → 中线变形绕行 (固定朝右)
HOLD_DECAY: 挡板消失 → 变形量线性衰减 (lidar_hold_frames 帧内归零) HOLD_DECAY: 挡板消失 → 变形量线性衰减 (lidar_hold_frames 帧内归零)
衰减期间若再次检测到挡板 → 立即切回 AVOID 衰减期间若再次检测到挡板 → 立即切回 AVOID
``` ```
| 状态 | 电机 | 舵机 | 说明 |
|------|------|------|------|
| NORMAL | 正常速度 | 正常巡线 | 无变形 |
| AVOID | speed × lidar_speed | 中线偏转绕行 | 基于 left/right 空间判断方向 |
| HOLD_DECAY | speed × lidar_speed | 变形量线性衰减 | 确保车完全通过后再恢复 |
--- ---
## 与现有流水线的集成 ## 速度联动
`lidar_avoid_process()` 放在 cone_detect_and_deform 之前执行（两者都修改 mid_line， `motor_update()` 中包含挡板减速代码（`lidar_is_active()` → `speed × lidar_speed`），
lidar 优先级更高）： 但当前**已注释**。绕行仅靠中线变形，不减速。如需启用，取消注释即可。
```
// CameraHandler 中 Step 5:
bool zebra_block = zebra_process();
bool tl_block = traffic_light_process();
lidar_avoid_process(); // ★ 新增：直接修改 mid_line
cone_detect_and_deform();
steering_update();
motor_update(zebra_block, tl_block);
```
### 中线修改的优先级
lidar 和 cone 都会修改 `mid_line[]`。由于 lidar 绕行是结构性避障（避开整个挡板），
应**先执行 lidar 变形，再执行 cone 变形**。
cone 的 clamp 到 `[left+2, right-2]` 会确保不超出 lidar 变形后的安全区间。
### motor_update 速度联动
```cpp
// motor_update 中新增：
if (g_lidar_confirmed || g_lidar_hold_ctr > 0) {
spd *= g_cfg.lidar_speed;
}
```
不通过 `block` 参数刹车（绕行不需要停车），而是降速 + 变形。
---
## 复用现有 VL53L0X 驱动
已有驱动（当前被 `CMakeLists.txt` 排除编译）：
```cpp
// lib/vl53l0x.h
VL53L0X sensor;
sensor.init(); // open /dev/stmvl53l0x_ranging
VL53L0X_RangingMeasurementData_t data;
sensor.readRange(data); // data.RangeMilliMeter
sensor.stop();
```
**集成时需做的事：**
1. 从 `CMakeLists.txt` 的 EXCLUDE 列表中移除 `vl53l0x.cpp`
2. 在 `CameraInit()` 中调用 `sensor.init()`（硬件未连接时优雅降级）
3. 在 `cameraDeInit()` 中调用 `sensor.stop()`
--- ---
## 配置参数 ## 配置参数
| 文件 | 类型 | 建议默认 | 含义 | | 文件 | 类型 | 默认 | 含义 |
|------|------|----------|------| |------|------|------|------|
| `./lidar_thresh` | int | 300 | 障碍判定距离阈值 (mm)，低于此值触发判定 | | `./lidar_enable` | int(0/1) | 1 | 激光避障总开关（0=禁用，init 也不调用） |
| `./lidar_thresh` | int | 300 | 障碍判定距离阈值 (mm)，低于此值触发确认 |
| `./lidar_pre` | int | 800 | 预触发窗口 (mm)，低于此值开始攒帧 |
| `./lidar_near` | int | 50 | row=lt_h-1 对应的物理距离 (mm) | | `./lidar_near` | int | 50 | row=lt_h-1 对应的物理距离 (mm) |
| `./lidar_far` | int | 1200 | row=10 对应的物理距离 (mm) | | `./lidar_far` | int | 1200 | row=10 对应的物理距离 (mm) |
| `./lidar_near_start` | int | 1 | near_valid 检测起始偏移 (row + N) |
| `./lidar_near_end` | int | 4 | near_valid 检测终止偏移 |
| `./lidar_far_span` | int | 6 | far_lost 检测跨度 (从 row 往上检查 N 行) |
| `./lidar_min_frames` | int | 3 | 连续确认帧数 | | `./lidar_min_frames` | int | 3 | 连续确认帧数 |
| `./lidar_avoid_gain` | double | 0.4 | 绕行中线推离力度 (归一化) | | `./lidar_avoid_gain` | double | 0.4 | 绕行中线推离力度 (归一化) |
| `./lidar_avoid_range` | int | 30 | 绕行斜坡陡峭度 (行数) | | `./lidar_avoid_range` | int | 30 | 绕行斜坡陡峭度 (行数) |
| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 | | `./lidar_speed` | double | 0.4 | 绕行时速度倍率 (当前未生效) |
| `./lidar_hold_frames` | int | 30 | 挡板消失后变形保持帧数 | | `./lidar_hold_frames` | int | 30 | 挡板消失后变形保持帧数 |
| `./lidar_enable` | int(0/1) | 1 | 激光避障总开关（0=禁用） | | `./lidar_near_start` | int | 1 | (已加载但未使用，预留) |
| `./lidar_near_end` | int | 4 | (已加载但未使用，预留) |
| `./lidar_far_span` | int | 6 | (已加载但未使用，预留) |
--- ---
## 边界情况 & 注意事项 ## 边界情况 & 注意事项
1. **激光硬件未连接** — `sensor.init()` 失败时，`lidar_avoid_process()` 直接返回 false，不阻塞正常行驶。 1. **激光硬件未连接** — `init()` 失败时 `g_lidar_ok = false`，后续跳过，不阻塞。
2. **弯道边墙误判** — 急弯处赛道边墙可能导致激光近距 + 边线丢失同时出现。依赖 `lidar_thresh` 和 `lidar_min_frames` 调参抑制。误判时车会短暂绕向一侧，弯道通过后立即恢复。 2. **弯道边墙误判** — 急弯处激光可能打到边墙产生近距读数。依赖 `lidar_thresh` + `lidar_min_frames` 去抖抑制。
3. **左右空间相等** — `left_space == right_space` 时默认 `dir = -1.0`（往右绕），可通过配置 `lidar_default_dir` 调整。 3. **方向固定右绕** — 当前不判断挡板在赛道左侧还是右侧，一律朝右推中线。对大多数场景够用，但若挡板在右侧则绕行方向不理想。
4. **上坡/下坡** — 车辆俯仰变化会影响距离→行的映射精度。建议在平坦路段标定。 4. **标定建议** — 在平坦路段用 debug 模式打印 `d_mm` 和 `row`，确认 `lidar_near` / `lidar_far` 映射准确。
5. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。 5. **VL53L0X 读取模式** — 非阻塞：先 `startMeasure()` 启动后台测量 (~20ms)，下次调用时 `readResult()` 取结果。每 2 帧一读不阻塞主循环。
6. **首次集成建议** — 先调通数据采集：打印 `d_mm`、对应 `row`、以及 `near_ref_row` 处的 `left/right/mid` 值，跑几圈确认标定参数无误后再开启绕行。 6. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。
7. **挡板材质** — 深色挡板不会被 HSV-Otsu 判为赛道（白色 255），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是**丢线**而非**像素颜色**。
docs/TRAFFIC_LIGHT.md
+17 -15
View File
@@ -2,11 +2,11 @@
## 1. 模型检测 ## 1. 模型检测
NanoDet V10 模型 4 类输出中： Mild v12 模型 4 类输出中：
- cls=1: 红灯 - cls=1: 红灯
- cls=2: 绿灯 - cls=2: 绿灯
模型置信度阈值由 `g_thresh[1]` 和 `g_thresh[2]` 控制（`src/camera.cpp:25`），当前分别为 0.80。 模型置信度阈值由 `g_thresh[1]` 和 `g_thresh[2]` 控制（`src/camera.cpp:42`），当前分别为 0.75（红灯）和 0.80（绿灯）。
## 2. 状态机 ## 2. 状态机
@@ -41,7 +41,8 @@ NanoDet V10 模型 4 类输出中：
和斑马线完全一样，复用编码器比例刹车： 和斑马线完全一样，复用编码器比例刹车：
``` ```
ControlUpdate(spd, g_zstate == Z_STOP || tl_block) motor_update(zebra_block, tl_block)
→ ControlUpdate(final_spd, zebra_block || tl_block)
刹车信号 = 斑马线STOP || 红灯STOP || 等待绿灯 刹车信号 = 斑马线STOP || 红灯STOP || 等待绿灯
``` ```
@@ -67,20 +68,21 @@ LCD 状态: "R"=红灯停车 "G"=等绿灯, 拼接斑马线状态
## 5. 关键代码路径 ## 5. 关键代码路径
``` ```
CameraHandler (src/camera.cpp) CameraHandler (src/camera.cpp:755)
Step 4: 模型推理 → g_boxes[], g_box_count Step 5: 模型推理 → g_boxes[], g_box_count
Step 5: 斑马线状态机 Step 6a: 斑马线状态机
Step 5.5: 红绿灯状态机 Step 6b: 红绿灯状态机
├─ 遍历 g_boxes, 统计 red_seen/green_seen ├─ 遍历 g_boxes, 统计 red_seen/green_seen
├─ 去抖计数 (3帧确认, 衰减-1/帧) ├─ 去抖计数 (3帧确认, 衰减-1/帧)
└─ 状态跃迁 └─ 状态跃迁
Step 5.6: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行) Step 6c: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行)
Step 7: 电机控制 Step 8: 电机控制
ControlUpdate(spd, g_zstate==Z_STOP || tl_block) motor_update(zebra_block, tl_block)
→ ControlUpdate(final_spd, zebra_block || tl_block)
control.cpp: control.cpp:
ControlUpdate(speed, zebra_block) ControlUpdate(speed, block)
└─ zebra_block → 编码器线程读速度 → 比例刹车 └─ block → 编码器线程读速度 → 比例刹车
``` ```
## 6. 状态变量 ## 6. 状态变量
lib/global.h
+10 -2
View File
@@ -12,6 +12,8 @@ const std::string start_file = "./start";
const std::string showImg_file = "./showImg"; const std::string showImg_file = "./showImg";
const std::string destfps_file = "./destfps"; const std::string destfps_file = "./destfps";
const std::string foresee_file = "./foresee"; const std::string foresee_file = "./foresee";
const std::string foresee_lost_scale_file = "./foresee_lost_scale";
const std::string sharp_turn_scale_file = "./sharp_turn_scale";
const std::string zebrasee_file = "./zebrasee"; const std::string zebrasee_file = "./zebrasee";
const std::string saveImg_file = "./saveImg"; const std::string saveImg_file = "./saveImg";
const std::string speed_file = "./speed"; const std::string speed_file = "./speed";
@@ -27,6 +29,8 @@ const std::string cone_min_frames_file = "./cone_min_frames";
const std::string cone_margin_file = "./cone_margin"; const std::string cone_margin_file = "./cone_margin";
const std::string cone_thresh_file = "./cone_thresh"; const std::string cone_thresh_file = "./cone_thresh";
const std::string cone_hold_frames_file = "./cone_hold_frames"; const std::string cone_hold_frames_file = "./cone_hold_frames";
const std::string cone_return_gain_file = "./cone_return_gain";
const std::string cone_return_frames_file = "./cone_return_frames";
const std::string brake_scale_file = "./brake_scale"; const std::string brake_scale_file = "./brake_scale";
const std::string brake_max_file = "./brake_max"; const std::string brake_max_file = "./brake_max";
@@ -56,6 +60,8 @@ void cfg_load_all();
struct CfgCache { struct CfgCache {
double speed = 60; // 目标速度(占空比 %) double speed = 60; // 目标速度(占空比 %)
double foresee = 80; // 前瞻行 double foresee = 80; // 前瞻行
double foresee_lost_scale = 0.7; // 丢线时前瞻缩放 (<1=看更远)
double sharp_turn_scale = 0.5; // 急弯时前瞻缩放 (<1=看更远)
double zebrasee = 60; // 斑马线触发距离阈值 double zebrasee = 60; // 斑马线触发距离阈值
double deadband = 5; // 舵机死区 double deadband = 5; // 舵机死区
double steer_gain = 1.0; // 舵机增益 double steer_gain = 1.0; // 舵机增益
@@ -67,10 +73,12 @@ struct CfgCache {
double cone_avoid_gain = 0.3; // 锥桶中线变形推离量 (归一化) double cone_avoid_gain = 0.3; // 锥桶中线变形推离量 (归一化)
int cone_avoid_range = 30; // 变形斜坡陡峭度 int cone_avoid_range = 30; // 变形斜坡陡峭度
double cone_speed = 0.5; // 锥桶触发时速度倍率 double cone_speed = 0.5; // 锥桶触发时速度倍率
int cone_min_frames = 3; // 连续确认帧数 int cone_min_frames = 1; // 确认帧数 (1=单帧即确认, 视觉巡线响应快导致检测窗口极短)
int cone_margin = 0; // 0=中心点 1=框边缘 int cone_margin = 0; // 0=中心点 1=框边缘
double cone_thresh = 0.80; // 锥桶置信度阈值 double cone_thresh = 0.80; // 锥桶置信度阈值
int cone_hold_frames = 30; // 锥桶消失后保持变形的帧数 int cone_hold_frames = 15; // 锥桶消失后保持变形的帧数
double cone_return_gain = 1.0; // 回弹力度 (倍乘, 1.0=等同于避让力度)
int cone_return_frames = 30; // 回弹持续帧数
double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数 double brake_scale = 10; // 速度(pps) → 刹车占空比(ns) 缩放系数
int brake_max = 10000; // 最大刹车占空比 (ns) int brake_max = 10000; // 最大刹车占空比 (ns)
lib/image_cv.h
+2
View File
@@ -12,5 +12,7 @@ extern cv::Mat track;
extern std::vector<int> left_line; extern std::vector<int> left_line;
extern std::vector<int> right_line; extern std::vector<int> right_line;
extern std::vector<int> mid_line; extern std::vector<int> mid_line;
extern std::vector<int> mid_line_raw;
extern int g_lost_rows;
extern int line_tracking_height, line_tracking_width; extern int line_tracking_height, line_tracking_width;
main/CMakeLists.txt
+4 -1
View File
@@ -3,4 +3,7 @@ add_executable(smartcar_demo1 main.cpp)
target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS}) target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS})
add_executable(lidar_test lidar_test.cpp) add_executable(lidar_test lidar_test.cpp)
target_link_libraries(lidar_test common_lib) target_link_libraries(lidar_test common_lib)
add_executable(remote_control remote_control.cpp)
target_link_libraries(remote_control common_lib)
main/remote_control.cpp
+93
View File
@@ -0,0 +1,93 @@
/*
* remote_control — 终端 WASD 遥控
*
* 用法: SSH 到小车后直接运行 ./remote_control
* W/S = 前进/后退
* A/D = 左转/右转
* Q = 退出
*
* 硬件:
* 左电机: pwmchip8/pwm2, GPIO12 方向, GPIO13 IN2=高
* 右电机: pwmchip8/pwm1, GPIO13 方向
* 使能: GPIO73
* 舵机: pwmchip1/pwm0, 3ms 周期, 1.5ms 中位
*/
#include "MotorController.h"
#include "PwmController.h"
#include "GPIO.h"
#include <cstdio>
#include <cstdlib>
#include <termios.h>
#include <unistd.h>
static constexpr unsigned int MOTOR_PERIOD_NS = 50000;
static constexpr unsigned int SERVO_PERIOD_NS = 3000000;
static constexpr unsigned int SERVO_CENTER_NS = 1500000;
static constexpr unsigned int SERVO_RANGE_NS = 300000;
int main()
{
GPIO mortorEN(73);
mortorEN.setDirection("out");
mortorEN.setValue(1);
GPIO leftIn2(13);
leftIn2.setDirection("out");
leftIn2.setValue(1);
MotorController motorL(8, 2, 12, MOTOR_PERIOD_NS);
MotorController motorR(8, 1, 13, MOTOR_PERIOD_NS);
PwmController servo(1, 0);
servo.setPeriod(SERVO_PERIOD_NS);
servo.setDutyCycle(SERVO_CENTER_NS);
servo.enable();
struct termios old_tio, new_tio;
tcgetattr(STDIN_FILENO, &old_tio);
new_tio = old_tio;
new_tio.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &new_tio);
printf("WASD 遥控已启动\n");
printf(" W=前进 S=后退 A=左转 D=右转 Q=退出 松手即停\n");
double speed = 0, steer = 0;
bool running = true;
while (running) {
fd_set fds;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
struct timeval tv = {0, 50000};
if (select(STDIN_FILENO + 1, &fds, nullptr, nullptr, &tv) > 0) {
char c;
if (read(STDIN_FILENO, &c, 1) == 1) {
switch (c) {
case 'w': case 'W': speed = 50; break;
case 's': case 'S': speed = -50; break;
case 'a': case 'A': steer = -100; break;
case 'd': case 'D': steer = 100; break;
case 'q': case 'Q': running = false; break;
}
}
} else {
speed = 0;
steer = 0;
}
unsigned int duty_ns = SERVO_CENTER_NS + (unsigned int)(steer / 100.0 * SERVO_RANGE_NS);
servo.setDutyCycle(duty_ns);
motorL.updateduty(speed);
motorR.updateduty(speed);
}
motorL.updateduty(0);
motorR.updateduty(0);
servo.setDutyCycle(SERVO_CENTER_NS);
mortorEN.setValue(0);
tcsetattr(STDIN_FILENO, TCSANOW, &old_tio);
printf("\n已退出\n");
return 0;
}
src/camera.cpp
+525 -159
View File
File diff suppressed because it is too large Load Diff
src/control.cpp
+70 -16
View File
@@ -1,8 +1,15 @@
/* /*
* control — 电机控制层 * control — 电机控制层
* *
* 开环占空比控制 + 斑马线编码器比例刹车。 * 开环占空比控制 + 编码器比例刹车（斑马线/红绿灯停车时触发）。
* 编码器线程: 高速轮询 gpio67 → 算速度(脉冲/秒) → 原子变量共享。 * 编码器线程: 高速轮询 gpio67 → 算速度(脉冲/秒) → 原子变量共享。
*
* 硬件映射:
* motor[0] = 左电机 pwmchip8/pwm2, GPIO12 方向
* motor[1] = 右电机 pwmchip8/pwm1, GPIO13 方向
* GPIO73 = 电机使能 (mortorEN)
* GPIO67 = 编码器 LSB 脉冲 (输入)
* GPIO72 = 编码器方向 (输入)
*/ */
#include "control.h" #include "control.h"
#include "global.h" #include "global.h"
@@ -10,17 +17,27 @@
#include <atomic> #include <atomic>
#include <cmath> #include <cmath>
// ═══════════════════════════════════════════════════════════
// 全局 & 静态变量
// ═══════════════════════════════════════════════════════════
MotorController *motorController[2] = {nullptr, nullptr}; MotorController *motorController[2] = {nullptr, nullptr};
GPIO mortorEN(73); GPIO mortorEN(73); // 电机使能引脚
static GPIO encoderLSB(67); static GPIO encoderLSB(67); // 编码器 LSB 脉冲
static GPIO encoderDIR(72); static GPIO encoderDIR(72); // 编码器方向
static std::thread g_enc_thread; static std::thread g_enc_thread;
static std::atomic<bool> g_enc_running{false}; static std::atomic<bool> g_enc_running{false};
static std::atomic<double> g_enc_speed{0.0}; static std::atomic<double> g_enc_speed{0.0}; // 编码器速度 (脉冲/秒)
static std::atomic<int> g_enc_dir{0}; static std::atomic<int> g_enc_dir{0}; // 编码器方向 (0/1)
// ═══════════════════════════════════════════════════════════
// 编码器线程 — 高频轮询测速
//
// ~2kHz (500µs sleep) 轮询 GPIO67 边沿计脉冲，
// 每 100ms 窗口计算一次速度写入原子变量 g_enc_speed。
// 仅在刹车时被 ControlUpdate 读取。
// ═══════════════════════════════════════════════════════════
static long enc_now_ns() { static long enc_now_ns() {
struct timespec ts; struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts); clock_gettime(CLOCK_MONOTONIC, &ts);
@@ -36,43 +53,58 @@ static void encoder_thread() {
int cur_lsb = encoderLSB.readValue() ? 1 : 0; int cur_lsb = encoderLSB.readValue() ? 1 : 0;
int cur_dir = encoderDIR.readValue() ? 1 : 0; int cur_dir = encoderDIR.readValue() ? 1 : 0;
// 检测 LSB 边沿变化 → 计脉冲
if (cur_lsb != prev_lsb) { if (cur_lsb != prev_lsb) {
pulse_cnt++; pulse_cnt++;
prev_lsb = cur_lsb; prev_lsb = cur_lsb;
} }
g_enc_dir.store(cur_dir); g_enc_dir.store(cur_dir);
// 每 100ms 窗口输出一次速度
long t1 = enc_now_ns(); long t1 = enc_now_ns();
long dt = t1 - t0; long dt = t1 - t0;
if (dt >= 100000000L) { if (dt >= 100000000L) { // 100ms
g_enc_speed.store(pulse_cnt / (dt / 1e9)); g_enc_speed.store(pulse_cnt / (dt / 1e9));
pulse_cnt = 0; pulse_cnt = 0;
t0 = t1; t0 = t1;
} }
// 500µs 间隔，防止 CPU 饿死其他线程
std::this_thread::sleep_for(std::chrono::microseconds(500)); std::this_thread::sleep_for(std::chrono::microseconds(500));
} }
} }
// ═══════════════════════════════════════════════════════════
// ControlInit — 初始化电机 + 编码器线程
//
// 调用时机: main.cpp 启动时，CameraInit 之后。
// ═══════════════════════════════════════════════════════════
void ControlInit() void ControlInit()
{ {
// 电机使能引脚
mortorEN.setDirection("out"); mortorEN.setDirection("out");
mortorEN.setValue(1); mortorEN.setValue(1);
// 编码器引脚
encoderLSB.setDirection("in"); encoderLSB.setDirection("in");
encoderDIR.setDirection("in"); encoderDIR.setDirection("in");
// 启动编码器测速线程
g_enc_running.store(true); g_enc_running.store(true);
g_enc_thread = std::thread(encoder_thread); g_enc_thread = std::thread(encoder_thread);
// GPIO13 (左电机 IN2) 设为高电平，配合 PWM 实现 H 桥正/反转
GPIO leftIn2(13); GPIO leftIn2(13);
leftIn2.setDirection("out"); leftIn2.setDirection("out");
leftIn2.setValue(1); leftIn2.setValue(1);
// 创建双电机控制器
// motor[0]: pwmchip8/pwm2 + GPIO12 方向 (左电机)
// motor[1]: pwmchip8/pwm1 + GPIO13 方向 (右电机)
const int pwmchip[2] = {8, 8}; const int pwmchip[2] = {8, 8};
const int pwmnum[2] = {2, 1}; const int pwmnum[2] = {2, 1};
const int gpioNum[2] = {12, 13}; const int gpioNum[2] = {12, 13};
const unsigned int period_ns = 50000; const unsigned int period_ns = 50000; // PWM 周期 50µs = 20kHz
for (int i = 0; i < 2; ++i) for (int i = 0; i < 2; ++i)
{ {
@@ -82,55 +114,77 @@ void ControlInit()
} }
} }
// ═══════════════════════════════════════════════════════════
// ControlUpdate — 每帧电机控制入口
//
// speed: 目标速度 (占空比 %)，已经过弯道减速处理
// zebra_block: true = 斑马线 STOP 或红绿灯 STOP/WAIT_GREEN
//
// 三种模式:
// 1. g_cfg.start == 0 → 全部停转，拉低使能
// 2. zebra_block → 编码器比例刹车 (反向 PWM)
// 3. 正常 → 开环占空比驱动
// ═══════════════════════════════════════════════════════════
void ControlUpdate(double speed, bool zebra_block) void ControlUpdate(double speed, bool zebra_block)
{ {
// ── 模式 1: 未启动 → 停转 ──
if (!g_cfg.start) if (!g_cfg.start)
{ {
for (int i = 0; i < 2; ++i) for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(0); if (motorController[i]) motorController[i]->updateduty(0);
mortorEN.setValue(0); mortorEN.setValue(0); // 拉低使能
return; return;
} }
// ── 模式 2: 刹车 (斑马线/红绿灯停车) ──
if (zebra_block) if (zebra_block)
{ {
double cur_speed = g_enc_speed.load(); double cur_speed = g_enc_speed.load(); // 原子读: 编码器速度 (pps)
int cur_dir = g_enc_dir.load(); int cur_dir = g_enc_dir.load(); // 原子读: 编码器方向
if (cur_speed > 0.5) if (cur_speed > 0.5) // 仍在运动
{ {
// 比例刹车: 速度越快 → 刹车占空比越大
int brake_ns = (int)(cur_speed * g_cfg.brake_scale); int brake_ns = (int)(cur_speed * g_cfg.brake_scale);
if (brake_ns > g_cfg.brake_max) brake_ns = g_cfg.brake_max; if (brake_ns > g_cfg.brake_max) brake_ns = g_cfg.brake_max;
double brake_pct = brake_ns / 500.0; // ns → % (period=50000ns) double brake_pct = brake_ns / 500.0; // ns → % (period=50000ns, 50000/100=500)
int dir = cur_dir ? 1 : 0; int dir = cur_dir ? 1 : 0;
// 施加反向 PWM: 方向取反实现刹车
for (int i = 0; i < 2; ++i) for (int i = 0; i < 2; ++i)
if (motorController[i]) if (motorController[i])
motorController[i]->updateduty(dir ? -brake_pct : brake_pct); motorController[i]->updateduty(dir ? -brake_pct : brake_pct);
} }
else else // 已停止 (< 0.5 pps)
{ {
// 不再施加刹车，防止反冲
for (int i = 0; i < 2; ++i) for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(0); if (motorController[i]) motorController[i]->updateduty(0);
} }
return; return;
} }
// ── 模式 3: 正常行驶 → 开环占空比 ──
for (int i = 0; i < 2; ++i) for (int i = 0; i < 2; ++i)
if (motorController[i]) motorController[i]->updateduty(speed); if (motorController[i]) motorController[i]->updateduty(speed);
mortorEN.setValue(1); mortorEN.setValue(1); // 使能拉高
} }
// ═══════════════════════════════════════════════════════════
// ControlExit — 停止编码器线程 + 释放电机资源
// ═══════════════════════════════════════════════════════════
void ControlExit() void ControlExit()
{ {
// 停止编码器线程
g_enc_running.store(false); g_enc_running.store(false);
if (g_enc_thread.joinable()) g_enc_thread.join(); if (g_enc_thread.joinable()) g_enc_thread.join();
// 释放电机控制器
for (int i = 0; i < 2; ++i) for (int i = 0; i < 2; ++i)
{ {
delete motorController[i]; delete motorController[i];
motorController[i] = nullptr; motorController[i] = nullptr;
} }
mortorEN.setValue(0); mortorEN.setValue(0); // 使能拉低
} }
src/global.cpp
+4
View File
@@ -24,6 +24,8 @@ void cfg_load_all()
{ {
g_cfg.speed = readDoubleFromFile(speed_file); g_cfg.speed = readDoubleFromFile(speed_file);
g_cfg.foresee = readDoubleFromFile(foresee_file); g_cfg.foresee = readDoubleFromFile(foresee_file);
g_cfg.foresee_lost_scale = readDoubleFromFile(foresee_lost_scale_file);
g_cfg.sharp_turn_scale = readDoubleFromFile(sharp_turn_scale_file);
g_cfg.zebrasee = readDoubleFromFile(zebrasee_file); g_cfg.zebrasee = readDoubleFromFile(zebrasee_file);
g_cfg.deadband = readDoubleFromFile(deadband_file); g_cfg.deadband = readDoubleFromFile(deadband_file);
g_cfg.steer_gain = readDoubleFromFile(steer_gain_file); g_cfg.steer_gain = readDoubleFromFile(steer_gain_file);
@@ -39,6 +41,8 @@ void cfg_load_all()
g_cfg.cone_avoid_gain = readDoubleFromFile(cone_avoid_gain_file); g_cfg.cone_avoid_gain = readDoubleFromFile(cone_avoid_gain_file);
g_cfg.cone_avoid_range = (int)readDoubleFromFile(cone_avoid_range_file); g_cfg.cone_avoid_range = (int)readDoubleFromFile(cone_avoid_range_file);
g_cfg.cone_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file); g_cfg.cone_hold_frames = (int)readDoubleFromFile(cone_hold_frames_file);
g_cfg.cone_return_gain = readDoubleFromFile(cone_return_gain_file);
g_cfg.cone_return_frames = (int)readDoubleFromFile(cone_return_frames_file);
g_cfg.brake_scale = readDoubleFromFile(brake_scale_file); g_cfg.brake_scale = readDoubleFromFile(brake_scale_file);
g_cfg.brake_max = readDoubleFromFile(brake_max_file); g_cfg.brake_max = readDoubleFromFile(brake_max_file);
src/image_cv.cpp
+10 -2
View File
@@ -30,6 +30,9 @@ cv::Mat track;
std::vector<int> left_line; std::vector<int> left_line;
std::vector<int> right_line; std::vector<int> right_line;
std::vector<int> mid_line; std::vector<int> mid_line;
std::vector<int> mid_line_raw;
int g_lost_rows = 0;
int line_tracking_width; // 处理宽度 = 80 int line_tracking_width; // 处理宽度 = 80
int line_tracking_height; // 处理高度 = 60 int line_tracking_height; // 处理高度 = 60
@@ -105,7 +108,8 @@ cv::Mat find_road(cv::Mat &frame)
cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel); cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);
static cv::Mat mask; static cv::Mat mask;
mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1); if (mask.empty()) mask.create(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
mask.setTo(0);
// 3. 种子点位置 // 3. 种子点位置
// X = 图像水平中心 (line_tracking_width/2) // X = 图像水平中心 (line_tracking_width/2)
@@ -130,7 +134,8 @@ cv::Mat find_road(cv::Mat &frame)
nullptr, loDiff, upDiff, 8); nullptr, loDiff, upDiff, 8);
static cv::Mat outputImage; static cv::Mat outputImage;
outputImage = cv::Mat::zeros(line_tracking_height, line_tracking_width, CV_8UC1); if (outputImage.empty()) outputImage.create(line_tracking_height, line_tracking_width, CV_8UC1);
outputImage.setTo(0);
mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage); mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage);
return outputImage; return outputImage;
@@ -244,11 +249,13 @@ void image_main()
// ── 6. 中线计算 + 丢线补全 ───── // ── 6. 中线计算 + 丢线补全 ─────
// 从倒数第二行开始向上，底行(row=height-1)单独兜底避免 mid_line[row+1] 越界 // 从倒数第二行开始向上，底行(row=height-1)单独兜底避免 mid_line[row+1] 越界
g_lost_rows = 0;
for (int row = line_tracking_height - 2; row >= 10; --row) for (int row = line_tracking_height - 2; row >= 10; --row)
{ {
// ── 6a. 丢线补全 ────────────────────────────── // ── 6a. 丢线补全 ──────────────────────────────
if (left_line[row] == -1 && right_line[row] == -1) if (left_line[row] == -1 && right_line[row] == -1)
{ {
g_lost_rows++;
// 当前行完全丢线: 用下行(row+1)的中线来虚拟补线 // 当前行完全丢线: 用下行(row+1)的中线来虚拟补线
mid_line[row] = mid_line[row + 1]; mid_line[row] = mid_line[row + 1];
@@ -278,6 +285,7 @@ void image_main()
const int row = line_tracking_height - 1; const int row = line_tracking_height - 1;
if (left_line[row] == -1 && right_line[row] == -1) if (left_line[row] == -1 && right_line[row] == -1)
{ {
g_lost_rows++;
mid_line[row] = line_tracking_width / 2; mid_line[row] = line_tracking_width / 2;
left_line[row] = 0; left_line[row] = 0;
right_line[row] = line_tracking_width - 1; right_line[row] = line_tracking_width - 1;