模型推理移至管道末尾降舵机延迟60→16ms + 视觉Mat复用 + 端到端延迟计时 + OpenCV 4.13 + WASD遥控
This commit is contained in:
spdis
@@ -3,7 +3,7 @@
|
||||
## Project overview
|
||||
|
||||
龙芯 2K0300 嵌入式自动驾驶智能车，OpenCV + Mild v12 魔改 YOLO 模型。
|
||||
板端运行，x86 Linux 交叉编译。
|
||||
板端运行，x86 Linux 交叉编译。No unit tests, no CI, no linting — tested on-device only.
|
||||
|
||||
## Build
|
||||
|
||||
@@ -17,30 +17,33 @@ make -j$(nproc)
|
||||
- **Cross-compilation target**: LoongArch64 (`-march=loongarch64 -mtune=loongarch64`)
|
||||
- **Toolchain**: `/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6`
|
||||
- **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().
|
||||
- Set `CROSS_COMPILE` to `0` in `cross.cmake` for native x86 builds.
|
||||
|
||||
## Project structure
|
||||
|
||||
| Directory | Purpose |
|
||||
|-----------|---------|
|
||||
| `src/` | Source → compiled into `common_lib` (static lib) |
|
||||
| `lib/` | Public headers only (no .cpp) |
|
||||
| `main/` | Entry point → executable `smartcar_demo1` |
|
||||
| `docs/` | Architecture docs (`ARCHITECTURE.md` is the design reference) |
|
||||
| `src/` | Source (.cpp + .hpp) → compiled into `common_lib` (static lib) |
|
||||
| `lib/` | Public headers (.h only) + `vl53l0x/` C API sources |
|
||||
| `main/` | Entry points → `smartcar_demo1` (main) + `lidar_test` (standalone test) |
|
||||
| `docs/` | Design docs (ARCHITECTURE, CONE_DESIGN, ENCODER_BRAKE, LIDAR_AVOID, TRAFFIC_LIGHT) |
|
||||
| `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
|
||||
|
||||
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)
|
||||
- `PIDController.cpp` — unused (motor is open-loop direct drive)
|
||||
- `serial.cpp` — Vofa/serial image streaming, unused
|
||||
|
||||
Note: `vl53l0x.cpp` IS compiled (lidar ranging is active).
|
||||
|
||||
## Device-side operation
|
||||
|
||||
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
|
||||
# On device:
|
||||
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
|
||||
```
|
||||
|
||||
`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
|
||||
|
||||
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
|
||||
- `./speed` (double) — target speed (duty cycle %)
|
||||
- `./deadband` — steering deadband (pixels)
|
||||
- `./steer_gain` — steering gain multiplier
|
||||
- `./center_bias` — midline offset correction
|
||||
- `./foresee` — look-ahead row for steering
|
||||
|
||||
### Debug / Display
|
||||
- `./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)
|
||||
Key groups:
|
||||
- **Steering**: `speed`, `deadband`, `steer_gain`, `center_bias`, `foresee`
|
||||
- **Debug**: `start` (0/1), `showImg` (0/1), `debug` (0/1), `destfps`, `saveImg`
|
||||
- **Zebra**: `zebrasee`
|
||||
- **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`
|
||||
- **Curve slowdown**: `curve_slope` (default 0.4), `curve_min` (default 0.8)
|
||||
- **Encoder brake**: `brake_scale`, `brake_max`
|
||||
- **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`
|
||||
|
||||
## Architecture
|
||||
|
||||
### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:578`)
|
||||
### Per-frame pipeline in `CameraHandler()` (`src/camera.cpp:755`)
|
||||
|
||||
CameraHandler 现已拆分为多个函数，主函数 ~40 行仅做编排：
|
||||
|
||||
```cpp
|
||||
```
|
||||
CameraHandler()
|
||||
├── capture_frame() // 1. MJPG 取帧 → 1/4解码 320×240 BGR
|
||||
├── save_image_if_requested() // 2. 保存帧 (debug)
|
||||
├── image_main() // 3. 视觉巡线 (80×60 HSV-Otsu-FloodFill)
|
||||
├── run_model_inference() // 4. Mild v12 每2帧推理 (160×120, 4类)
|
||||
├── zebra_process() // 5a. 斑马线去抖 + Z_STOP(4s)/Z_COOLDOWN(5s)
|
||||
├── traffic_light_process() // 5b. 红绿灯 TL_NORMAL→TL_STOP→TL_WAIT_GREEN
|
||||
├── cone_detect_and_deform() // 5c. 锥桶检测 + 中线变形 + 保持衰减
|
||||
├── steering_update() // 6. 舵机比例控制 (deadband过滤)
|
||||
├── motor_update() // 7. 电机开环PWM + 编码器刹车 + 弯道减速
|
||||
├── lcd_render() // 8. LCD RGB565渲染 (边界线+检测框+状态)
|
||||
└── fps_log() // 9. 每15帧打印分步耗时
|
||||
├── lidar_avoid_process() // 4. VL53L0X 激光避障 (每2帧) + 中线变形
|
||||
├── run_model_inference() // 5. Mild v12 每2帧推理 (160×120, 4类)
|
||||
├── zebra_process() // 6a. 斑马线去抖 + Z_STOP/Z_COOLDOWN
|
||||
├── traffic_light_process() // 6b. 红绿灯 TL_NORMAL→TL_STOP→TL_WAIT_GREEN
|
||||
├── cone_detect_and_deform() // 6c. 锥桶检测 + 中线变形 + 保持衰减
|
||||
├── steering_update() // 7. 舵机比例控制 (deadband过滤)
|
||||
├── motor_update() // 8. 电机开环PWM + 编码器刹车 + 弯道减速
|
||||
├── 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.
|
||||
- **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.
|
||||
- **Curve slowdown**: `speed *= (1.0 - |deviation| × 0.4)`, clamped to ≥ 60%.
|
||||
### Motor control
|
||||
|
||||
- **Open-loop**: `MotorController::updateduty(spd)` — direct PWM, no encoder feedback in normal driving.
|
||||
- **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
|
||||
|
||||
- **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 (cone / red light / green light / zebra) + background
|
||||
- **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)
|
||||
- **Input**: 160×120 BGR, **Output**: 4 classes + background
|
||||
- **Class indices**: 0=cone, 1=red light, 2=green light, 3=zebra
|
||||
- **Per-class thresholds**: `{0.65, 0.75, 0.80, 0.82}` (cone/red/green/zebra)
|
||||
- **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)
|
||||
|
||||
### 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
|
||||
- **Inference engine**: `src/model_v10.{hpp,cpp}` (file names say v10, internally Mild v3 — historical naming)
|
||||
|
||||
## Style and conventions
|
||||
|
||||
- C++ files have **no copyright headers** — comments are ascii-box-style block comments when present
|
||||
- Header guards use `#ifndef FILENAME_H_` / `#define FILENAME_H_` format (with some exceptions: `#pragma once` in model files)
|
||||
- Global state uses `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`)
|
||||
- `lib/` contains .h files only; `src/` contains .cpp files only
|
||||
- No unit tests, no CI, no linting — this is embedded code tested on-device
|
||||
- No copyright headers — comments use ascii-box-style block dividers
|
||||
- Header guards: `#ifndef FILENAME_H_` / `#define FILENAME_H_` (some model files use `#pragma once`)
|
||||
- Global state via `extern` globals (e.g., `g_cfg`, `g_steer_deviation`, `g_boxes`)
|
||||
- `lib/` has `.h` headers; `src/` has `.cpp` implementations + `.hpp` model headers
|
||||
- No unit tests, no CI, no linting — do not add comments to code unless asked
|
||||
|
||||
+2
-1
@@ -7,13 +7,14 @@ cmake_minimum_required(VERSION 3.5.0)
|
||||
# 设置 C++ 标准
|
||||
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_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")
|
||||
|
||||
# 定义项目名称和版本
|
||||
project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX)
|
||||
|
||||
# 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)
|
||||
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||
message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
|
||||
|
||||
@@ -36,12 +36,14 @@ init_pins() {
|
||||
echo 8 > "$DIR/deadband" 2>/dev/null
|
||||
echo 1.5 > "$DIR/steer_gain" 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.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 1 > "$DIR/cone_min_frames" 2>/dev/null
|
||||
echo 0 > "$DIR/cone_margin" 2>/dev/null
|
||||
echo 0.80 > "$DIR/cone_thresh" 2>/dev/null
|
||||
echo 30 > "$DIR/cone_hold_frames" 2>/dev/null
|
||||
|
||||
+51
-29
@@ -34,13 +34,12 @@
|
||||
```
|
||||
main()
|
||||
├── cfg_load_all() # 从工作目录文件读取全部配置
|
||||
├── 补读 cone_avoid_gain/range/hold_frames
|
||||
├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, 计算巡线尺寸
|
||||
├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, VL53L0X, 计算巡线尺寸
|
||||
├── ControlInit() # 初始化双电机 GPIO + PWM + 启动编码器线程
|
||||
│
|
||||
└── while(running):
|
||||
├── CameraHandler() # ★ 逐帧执行
|
||||
└── target_speed = g_cfg.speed # (debug 模式下每 7 帧重载)
|
||||
└── target_speed = g_cfg.speed # (debug 模式下每 30 帧重载)
|
||||
```
|
||||
|
||||
`CameraInit` 现在只接受 `camera_id` 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。
|
||||
@@ -48,7 +47,7 @@ main()
|
||||
|
||||
---
|
||||
|
||||
## CameraHandler 11 步流水线
|
||||
## CameraHandler 10 步流水线 (`src/camera.cpp:755`)
|
||||
|
||||
```
|
||||
Step 1 capture_frame() 640×480 MJPEG → IMREAD_REDUCED_COLOR_4
|
||||
@@ -62,36 +61,43 @@ Step 3 image_main() raw_frame → resize(lt_w×lt_h) → HSV 双
|
||||
视觉巡线 → floodFill(种子 (lt_w/2, lt_h-10), 半径5)
|
||||
→ 逐行最长连续段搜索 → 丢线补全(row 10~59)
|
||||
输出: 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()
|
||||
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
|
||||
|
||||
Step 5a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s)
|
||||
Step 6a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s)
|
||||
斑马线状态机 触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
|
||||
刹停时 I2C 语音播报
|
||||
|
||||
Step 5b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN
|
||||
Step 6b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN
|
||||
红绿灯状态机 红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行
|
||||
|
||||
Step 5c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减
|
||||
锥桶检测 & 中线变形 cone_speed 减速 + cone_hold_frames 保持
|
||||
Step 6c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减 + 回弹
|
||||
锥桶检测 & 中线变形 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
|
||||
|
||||
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
|
||||
|
||||
Step 9 fps_log() 每 15 帧输出总帧率 + 分步耗时 ms
|
||||
Step 10 fps_log() 分步耗时日志
|
||||
FPS 统计
|
||||
```
|
||||
|
||||
另：每 150 帧调用 `malloc_trim(0)` 回收空闲内存（嵌入式优化）。
|
||||
|
||||
---
|
||||
|
||||
## 视觉巡线深度展开 (`image_main`)
|
||||
@@ -183,9 +189,10 @@ ControlUpdate(speed, zebra_or_tl_block):
|
||||
motor[i]->updateduty(0) ← 已静止，不刹车
|
||||
return
|
||||
|
||||
curve = 1.0 - |g_steer_deviation| × 0.4 ← 弯道减速
|
||||
curve = max(curve, 0.6) ← 最低 ≥ 60%
|
||||
curve = 1.0 - |g_steer_deviation| × g_cfg.curve_slope ← 弯道减速 (默认 0.4)
|
||||
curve = max(curve, g_cfg.curve_min) ← 最低倍率 (默认 0.8)
|
||||
spd = speed × curve
|
||||
// 锥桶减速 (cone_speed) / 挡板减速 (lidar_speed) 代码存在但已注释
|
||||
|
||||
motor[0].updateduty(spd) → 左电机 (pwmchip8/pwm2, gpio12 方向)
|
||||
motor[1].updateduty(spd) → 右电机 (pwmchip8/pwm1, gpio13 方向)
|
||||
@@ -196,8 +203,8 @@ ControlUpdate(speed, zebra_or_tl_block):
|
||||
刹车力度与当前车速成正比（`brake_scale`），有上限（`brake_max` ns）。
|
||||
速度低于 0.5 pps 时不再施加刹车。
|
||||
|
||||
**motor_update 额外逻辑：** `motor_update()` 在调用 `ControlUpdate` 之前，若锥桶已触发（`cone_is_slow()`），
|
||||
会将 speed 乘以 `g_cfg.cone_speed`（默认 0.5），实现锥桶路段减速。
|
||||
**motor_update 额外逻辑：** `motor_update()` 中包含锥桶和挡板减速代码（`cone_is_slow()` → `× cone_speed`，
|
||||
`lidar_is_active()` → `× lidar_speed`），但当前**已注释**，仅弯道减速生效。
|
||||
|
||||
**motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。
|
||||
|
||||
@@ -347,28 +354,43 @@ lcd_render():
|
||||
|---|---|---|---|---|
|
||||
| `./speed` | double | 60 | 11 | 目标速度 (% 占空比) |
|
||||
| `./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） |
|
||||
| `./foresee` | double | 80 | 40 | 舵机前瞻行 (显示空间像素) |
|
||||
| `./deadband` | double | 5 | 8 | 舵机死区 (显示空间像素) |
|
||||
| `./steer_gain` | double | 1.0 | 1.5 | 舵机增益 |
|
||||
| `./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) |
|
||||
| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
|
||||
| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
|
||||
| `./cone_avoid_gain` | double | 0.3 | 0.3 | 锥桶中线变形推离量 (归一化) |
|
||||
| `./cone_avoid_range` | int | 30 | 30 | 变形斜坡陡峭度 |
|
||||
| `./cone_speed` | double | 0.5 | 0.5 | 锥桶触发时速度倍率 |
|
||||
| `./cone_min_frames` | int | 3 | 3 | 锥桶连续确认帧数 |
|
||||
| `./cone_speed` | double | 0.5 | 0.5 | 锥桶触发时速度倍率 (当前未生效) |
|
||||
| `./cone_min_frames` | int | 1 | 1 | 锥桶确认帧数 (单帧即确认) |
|
||||
| `./cone_margin` | int | 0 | 0 | 0=中心点, 1=框边缘检查 |
|
||||
| `./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_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` 写入的值覆盖代码默认值，是实际运行参数。
|
||||
|
||||
@@ -380,12 +402,12 @@ lcd_render():
|
||||
|
||||
| 文件 | 功能 | 排除原因 |
|
||||
|---|---|---|
|
||||
| `vl53l0x.cpp` | 激光测距模块 | 硬件未连接 |
|
||||
| `zebra_detect.cpp` | 经典斑马线检测 | 已被 Mild 模型替代 |
|
||||
| `PIDController.cpp` | 位置式/增量式 PID | 电机开环直驱，无调用点 |
|
||||
| `serial.cpp` | VOFA 串口可视化 | 未使用 |
|
||||
|
||||
**注意：** `PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比），
|
||||
**注意：** `vl53l0x.cpp` 已恢复编译（激光测距已集成至 `lidar_avoid_process()`）。
|
||||
`PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比），
|
||||
不提供 `updateSpeed()` 方法。
|
||||
|
||||
---
|
||||
@@ -396,7 +418,7 @@ lcd_render():
|
||||
- **输入**：160×120 BGR
|
||||
- **输出**：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)
|
||||
- **阈值**：`{0.90, 0.75, 0.80, 0.90}`（锥桶/红灯/绿灯/斑马线）
|
||||
- **阈值**：`{0.65, 0.75, 0.80, 0.82}`（锥桶/红灯/绿灯/斑马线）
|
||||
- **权重文件**：`mild_v12.bin`（105KB 自定义二进制格式）
|
||||
- **推理频率**：每 2 帧一次（跳帧节省 CPU）
|
||||
|
||||
|
||||
+169
-79
@@ -4,7 +4,27 @@
|
||||
|
||||
**不改舵机代码，直接修改 `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. 去抖机制
|
||||
|
||||
与之前一致，不改变：
|
||||
|
||||
```
|
||||
#define CONE_MIN_FRAMES 3
|
||||
#define CONE_COUNT_DECAY 1
|
||||
|
||||
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)
|
||||
```
|
||||
|
||||
**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。
|
||||
- 锥桶在 → 变形在（每帧重新计算 mid_line 变形）
|
||||
- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失
|
||||
- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑
|
||||
**cone_min_frames 默认值为 1（单帧即确认）。** 原因：视觉巡线（`image_main()` 的 floodFill）
|
||||
对赛道上的锥桶响应极快——锥桶进入视野后 1~2 帧内车就开始转弯，锥桶随即离开视野。
|
||||
模型检测的窗口天然只有 1~2 帧，多帧去抖门槛（如 3）永远达不到。
|
||||
边界过滤（row 有效、赛道内、conf ≥ cone_thresh）已经提供了足够的误触发保护。
|
||||
|
||||
---
|
||||
|
||||
## 7. 舵机与电机
|
||||
## 7. 保持衰减 + 回弹
|
||||
|
||||
**舵机：** 零改动。Step 6 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。
|
||||
锥桶消失后不会立刻恢复正常巡线，而是经历三个阶段：
|
||||
|
||||
**电机：** 推入期间减速：
|
||||
```
|
||||
if (g_cone_confirmed && g_zstate != Z_STOP) {
|
||||
effective_speed = target_speed * cone_speed_factor;
|
||||
} else {
|
||||
effective_speed = target_speed;
|
||||
确认 (g_cone_confirmed && cone_seen)
|
||||
→ g_cone_hold_ctr = 1, g_cone_return_ctr = 0
|
||||
→ 冻结位置: hold_src_row/col = cone_row/col
|
||||
→ 记录回弹方向: 锥桶在中线左侧 → 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_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) |
|
||||
|
||||
### 8.2 热更新参数
|
||||
|
||||
| 文件 | 类型 | 默认 | 含义 |
|
||||
|---|---|---|---|
|
||||
| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 |
|
||||
| `./cone_min_frames` | int | 3 | 连续确认帧数 |
|
||||
| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 (当前减速代码已注释) |
|
||||
| `./cone_min_frames` | int | 1 | 确认帧数 (1=单帧即确认) |
|
||||
| `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 |
|
||||
| `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) |
|
||||
|
||||
### 8.3 读取实现
|
||||
|
||||
**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
|
||||
```
|
||||
| `./cone_hold_frames` | int | 15 | 消失后保持变形帧数 |
|
||||
| `./cone_return_gain` | double | 1.0 | 回弹力度 (1.0=等同避让力度) |
|
||||
| `./cone_return_frames` | int | 30 | 回弹持续帧数 |
|
||||
|
||||
---
|
||||
|
||||
## 9. 代码修改清单
|
||||
## 10. 代码位置
|
||||
|
||||
```
|
||||
lib/image_cv.h:
|
||||
└── extern mid_line_raw (原始中线快照声明)
|
||||
|
||||
src/image_cv.cpp:
|
||||
└── std::vector<int> mid_line_raw (定义)
|
||||
|
||||
lib/global.h:
|
||||
├── CfgCache 新增: cone_avoid_gain, cone_avoid_range,
|
||||
│ cone_speed, cone_min_frames, cone_margin, cone_thresh
|
||||
└── 新增文件名常量
|
||||
└── CfgCache 包含: cone_avoid_gain, cone_avoid_range, cone_speed,
|
||||
cone_min_frames, cone_margin, cone_thresh,
|
||||
cone_hold_frames, cone_return_gain, cone_return_frames
|
||||
|
||||
src/global.cpp:
|
||||
└── cfg_load_all() 追加热更新参数 (不含 avoid_gain/avoid_range)
|
||||
|
||||
main/main.cpp:
|
||||
└── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg
|
||||
└── cfg_load_all() 统一读取所有锥桶参数
|
||||
|
||||
src/camera.cpp:
|
||||
├── #define CONE_CLASS 0
|
||||
├── 新增静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed
|
||||
├── Step 5 之后: 插入锥桶检测+边界过滤+去抖
|
||||
├── Step 5.5: 中线变形 (修改 mid_line[row] for row in [10, cone_row])
|
||||
├── Step 7 电机: g_cone_confirmed → effective_speed *= cone_speed_factor
|
||||
└── Step 8 LCD: 锥桶框改橙色
|
||||
├── CameraHandler(): image_main() 后执行 mid_line_raw = mid_line (快照)
|
||||
├── 静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed,
|
||||
│ g_cone_hold_ctr, g_cone_hold_src_row/col,
|
||||
│ g_cone_return_ctr, g_cone_return_dir
|
||||
├── cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段
|
||||
├── cone_detect_and_deform(): 方向判断基于 mid_line_raw, 变形修改 mid_line
|
||||
├── motor_update(): cone_speed 减速 (已注释)
|
||||
└── lcd_render(): 锥桶框橙色绘制
|
||||
|
||||
ctl.sh:
|
||||
└── init_pins() 追加 6 个新文件默认值
|
||||
└── init_pins() 写入 cone_avoid_gain/range/speed/min_frames/margin/thresh/hold_frames
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 10. 方案对比
|
||||
## 11. 方案对比
|
||||
|
||||
| | 旧方案（舵机推入） | 新方案（中线变形） |
|
||||
| | 旧方案（舵机推入） | 当前方案（中线变形） |
|
||||
|---|---|---|
|
||||
| 舵机代码 | 需改动 | 零改动 |
|
||||
| deadband | 需绕过 | 自然生效 |
|
||||
| steer_gain | 需单独 cone_gain | 共用 |
|
||||
| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) |
|
||||
| 归还机制 | 需计时器+衰减 | hold 衰减 + return 回弹 |
|
||||
| 平滑性 | 需 EMA | 斜坡自带平滑 |
|
||||
| 参数数量 | 7 | 6 |
|
||||
| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 |
|
||||
| 参数数量 | 7 | 9 (含 hold/return) |
|
||||
| 锥桶消失恢复 | 需手动衰减 | 自动: hold 衰减 → return 回弹 → 正常 |
|
||||
|
||||
+84
-57
@@ -2,7 +2,7 @@
|
||||
|
||||
## 1. 硬件
|
||||
|
||||
编码器2，引脚：
|
||||
编码器引脚：
|
||||
|
||||
| 信号 | 引脚 | 模式 |
|
||||
|---|---|---|
|
||||
@@ -11,77 +11,99 @@
|
||||
|
||||
两引脚均属 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。
|
||||
|
||||
## 3. 刹车状态机
|
||||
`ControlInit()` 启动独立线程 `encoder_thread()`（`src/control.cpp:30`），持续运行直到 `ControlExit()`。
|
||||
|
||||
```
|
||||
┌──────────┐
|
||||
正常行驶 ───→ │ IDLE │
|
||||
└────┬─────┘
|
||||
│ zebra_block==true
|
||||
▼
|
||||
┌──────────┐
|
||||
│ BRAKING │ ← 施加 -30% 占空比
|
||||
│ │ 每帧读 GPIO 67
|
||||
└────┬─────┘
|
||||
┌───────────┴───────────┐
|
||||
│ 脉冲变化 │ 脉冲不变
|
||||
▼ ▼
|
||||
轮子在转 → 继续刹车 g_brake_still++
|
||||
重置计数器 │
|
||||
连续 5 帧无变化？
|
||||
或超时 60 帧？
|
||||
│
|
||||
▼
|
||||
┌──────────┐
|
||||
│ HOLDING │ ← duty=0，保持静止
|
||||
└──────────┘
|
||||
│
|
||||
zebra_block==false
|
||||
│
|
||||
▼
|
||||
┌──────────┐
|
||||
│ IDLE │
|
||||
└──────────┘
|
||||
encoder_thread():
|
||||
高频轮询 GPIO67 (LSB 边沿) + GPIO72 (方向)
|
||||
轮询间隔: 500µs sleep (~2kHz)
|
||||
|
||||
每 100ms 窗口:
|
||||
g_enc_speed = pulse_cnt / elapsed_time (脉冲/秒, 原子变量)
|
||||
g_enc_dir = GPIO72 当前值 (原子变量)
|
||||
pulse_cnt 归零, 开始新窗口
|
||||
```
|
||||
|
||||
## 4. 参数
|
||||
通过 sysfs 文件描述符轮询（`GPIO::readValue()`），`lseek` + `read`。不依赖中断。
|
||||
|
||||
定义在 `src/control.cpp`：
|
||||
## 3. 触发条件
|
||||
|
||||
| 常量 | 值 | 含义 |
|
||||
|---|---|---|
|
||||
| `BRAKE_STILL_THRESH` | 5 | 连续无脉冲帧数阈值，超过即判停 |
|
||||
| `BRAKE_TIMEOUT` | 60 | 最长刹车帧数（~2s @30fps），强制释放 |
|
||||
| `BRAKE_DUTY` | -30 | 刹车占空比（-100~100，负值=反转） |
|
||||
`ControlUpdate(speed, block)` 中 `block == true` 时进入刹车流程。
|
||||
|
||||
## 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）
|
||||
- 锥桶避障不使用编码器
|
||||
- `g_cfg.start == 0` 时刹车状态机重置
|
||||
| 文件 | 类型 | 默认 | 含义 |
|
||||
|---|---|---|---|
|
||||
| `./brake_scale` | double | 10 | 速度(pps) → 刹车占空比(ns) 缩放系数 |
|
||||
| `./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. 关键代码路径
|
||||
|
||||
@@ -89,7 +111,12 @@ motor[i]->updateduty(-30)
|
||||
main.cpp
|
||||
└── while(running)
|
||||
└── CameraHandler()
|
||||
└── ControlUpdate(target_speed, g_zstate == Z_STOP)
|
||||
└── if (zebra_block) → 刹车状态机
|
||||
└── encoderLSB.readValue() ← GPIO 67
|
||||
├── zebra_process() → zebra_block
|
||||
├── traffic_light_process() → tl_block
|
||||
└── 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
|
||||
```
|
||||
|
||||
+97
-253
@@ -1,232 +1,130 @@
|
||||
# 激光雷达挡板避障设计
|
||||
|
||||
## 问题
|
||||
## 概述
|
||||
|
||||
VL53L0X 是单点 ToF 激光测距传感器，仅返回沿光束方向的一个距离值（mm），没有扇形扫描能力。
|
||||
当激光测到近处有物体时，无法直接从距离值判断它是：
|
||||
VL53L0X 单点 ToF 激光测距传感器，返回沿光束方向的距离值（mm）。
|
||||
当距离低于阈值时触发绕行：中线变形推向右侧，消失后保持衰减。
|
||||
|
||||
- A) **挡板/障碍物** — 横在赛道上的物体，需要绕行
|
||||
- B) **赛道边墙** — 弯道处车头对准了侧墙，这是正常行驶状态
|
||||
|
||||
**解决思路：用视觉巡线的边界线（left_line / right_line）来区分 A 和 B。**
|
||||
**当前实现：** 纯距离触发 + 预触发去抖，不检查视觉边界线。绕行方向固定朝右。
|
||||
|
||||
---
|
||||
|
||||
## 关键约束：挡板会导致丢线
|
||||
## VL53L0X 驱动集成
|
||||
|
||||
挡板立在赛道上时，不仅触发激光近距读数，还会**遮挡摄像头视野中的赛道边界**，
|
||||
导致从挡板所在位置开始边界线丢失（left_line / right_line = -1）。
|
||||
驱动已编译（`src/vl53l0x.cpp` + `lib/vl53l0x/` C API），在 `CameraInit()` 中初始化：
|
||||
|
||||
```
|
||||
Camera 俯视视角 (图像坐标):
|
||||
row 0 (远) : ░░░░░░░ ← 被挡板挡住，丢线
|
||||
row 15 : ░░░░░░░ ← 挡板上方，丢线
|
||||
row 25 : ░░░░░░░ ← 挡板顶部附近，丢线
|
||||
row 30 : ███████ ← ★ 挡板所在行，边界线丢失
|
||||
row 35 : ■■■■■■■ ← 挡板下方，赛道可见，边界线有效
|
||||
row 59 (近) : ■■■■■■■ ← 车前方，赛道清晰
|
||||
CameraInit():
|
||||
if g_cfg.lidar_enable:
|
||||
g_lidar_ok = g_lidar_sensor.init() // open /dev/stmvl53l0x_ranging
|
||||
g_lidar_sensor.startMeasure() // 启动首次测量
|
||||
else:
|
||||
g_lidar_ok = false // 禁用
|
||||
|
||||
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
|
||||
｜ (高度 H, 俯角 θ)
|
||||
｜╲
|
||||
｜ ╲ laser beam
|
||||
｜ ╲
|
||||
ground ──────────────┴────███████──── 挡板 at distance d_mm
|
||||
(挡板后方赛道被遮挡)
|
||||
Step 3 image_main() → left_line[], right_line[], mid_line[]
|
||||
Step 4 lidar_avoid_process() → 修改 mid_line[] (绕行变形) ★
|
||||
Step 5 run_model_inference()
|
||||
Step 6c cone_detect_and_deform() → 也修改 mid_line[] (叠加在 lidar 之上)
|
||||
```
|
||||
|
||||
- 激光光束沿车体正前方（图像中轴线）
|
||||
- 距离 d_mm 越小 → 物体越近 → 映射到图像中越靠下的行（row 大）
|
||||
- 距离 d_mm 越大 → 物体越远 → 映射到图像中越靠上的行（row 小）
|
||||
lidar 先于 cone 修改 mid_line，优先级更高。cone 的 clamp 确保不超出边界。
|
||||
|
||||
---
|
||||
|
||||
## 核心算法
|
||||
|
||||
### 第一步：读取激光距离
|
||||
### 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:
|
||||
return CLEAR // 远处无障碍，不做任何处理
|
||||
data = g_lidar_sensor.readResult() // 非阻塞读取上次测量结果
|
||||
d_mm = data.RangeMilliMeter
|
||||
g_lidar_sensor.startMeasure() // 立即启动下次测量 (后台 ~20ms)
|
||||
```
|
||||
|
||||
`LIDAR_THRESHOLD_MM` 是触发阈值。只有距离小于此值才进入判定。建议默认 ~300mm。
|
||||
|
||||
### 第二步：距离 → 图像行映射
|
||||
### 2. 预触发去抖
|
||||
|
||||
```
|
||||
// 线性模型:
|
||||
// row = lt_h-1 (底行, 最近) 对应 D_NEAR
|
||||
// row = 10 (最远有效行) 对应 D_FAR
|
||||
// clamp 到 [10, lt_h-1]
|
||||
if d_mm == 0: // 读失败, 跳过
|
||||
skip
|
||||
|
||||
row = lt_h - 1 - (d_mm - D_NEAR) / (D_FAR - D_NEAR) * (lt_h - 11)
|
||||
if d_mm >= g_cfg.lidar_pre: // 距离超出预触发窗口 (默认 800mm)
|
||||
g_lidar_frames = max(0, frames-1) // 衰减但不归零
|
||||
skip
|
||||
|
||||
// d_mm < lidar_pre → 进入触发判定
|
||||
```
|
||||
|
||||
`lidar_pre`（默认 800mm）是预触发窗口：距离进入此范围就开始攒帧，但要 < `lidar_thresh`（默认 300mm）才真正确认。
|
||||
|
||||
### 3. 距离 → 图像行映射
|
||||
|
||||
```
|
||||
row = lt_h - 1 - (d_mm - lidar_near) × (lt_h - 11) / (lidar_far - lidar_near)
|
||||
row = clamp(row, 10, lt_h - 1)
|
||||
```
|
||||
|
||||
**标定值（需根据实际安装位置测量）：**
|
||||
| 参数 | 默认 | 含义 |
|
||||
|------|------|------|
|
||||
| `lidar_near` | 50 mm | row = lt_h-1（最近行）对应的距离 |
|
||||
| `lidar_far` | 1200 mm | row = 10（最远有效行）对应的距离 |
|
||||
|
||||
| 参数 | 含义 | 建议初值 |
|
||||
|------|------|----------|
|
||||
| `D_NEAR` | row = lt_h-1 对应的物理距离 | 50 mm |
|
||||
| `D_FAR` | row = 10 对应的物理距离 | 1200 mm |
|
||||
|
||||
标定方法：在赛道前方 300mm、600mm、900mm 处各放一个挡板，记录图像中挡板出现的 row，线性拟合。
|
||||
|
||||
### 第三步：用边线判定障碍物（核心）
|
||||
### 4. 触发判定
|
||||
|
||||
```
|
||||
// 设 row = distance_to_row(d_mm)，即激光测距对应的图像行
|
||||
|
||||
// ── 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
|
||||
```
|
||||
|
||||
**判定原理（三种典型场景）：**
|
||||
|
||||
```
|
||||
场景 A: 挡板挡路 → 触发绕行
|
||||
d_mm = 300mm → row = 35
|
||||
row 36~39 (近处): ✓ 赛道可见
|
||||
row 30~35 (挡板处): ✗ 丢线 (挡板遮挡)
|
||||
→ near_valid=true, far_lost=true → ★ 触发
|
||||
|
||||
场景 B: 正常直道，无障碍
|
||||
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
|
||||
```
|
||||
|
||||
### 第四步：去抖确认
|
||||
|
||||
```
|
||||
if OBSTACLE_CONFIRMED:
|
||||
g_lidar_frames++
|
||||
else:
|
||||
g_lidar_frames = max(0, g_lidar_frames - 1)
|
||||
g_lidar_obstacle_row = row
|
||||
|
||||
g_lidar_confirmed = (g_lidar_frames >= LIDAR_MIN_FRAMES)
|
||||
g_lidar_confirmed = (d_mm < lidar_thresh) && (g_lidar_frames >= lidar_min_frames)
|
||||
```
|
||||
|
||||
---
|
||||
**注意：** 不检查视觉边界线（near_valid / far_lost），纯距离触发。
|
||||
配置中的 `lidar_near_start`、`lidar_near_end`、`lidar_far_span` 参数被加载但**未使用**（为设计预留）。
|
||||
|
||||
## 绕行策略：中线变形
|
||||
|
||||
挡板通常只挡赛道的一部分（偏左或偏右），通过判断挡板下方有效行中哪一侧空间更大，
|
||||
将中线推向宽侧实现绕行。
|
||||
|
||||
### 判断绕行方向
|
||||
### 5. 确认 → 记录状态
|
||||
|
||||
```
|
||||
// 在 near_ref_row（挡板下方最近的有效行）中判断左右空间
|
||||
left_space = mid_line[near_ref_row] - left_line[near_ref_row]
|
||||
right_space = right_line[near_ref_row] - mid_line[near_ref_row]
|
||||
|
||||
// 往宽侧推
|
||||
dir = (left_space > right_space) ? +1.0 : -1.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
|
||||
g_lidar_hold_dir = 1.0 // 固定朝右绕行
|
||||
g_lidar_frames = 0 // 重置计数
|
||||
```
|
||||
|
||||
// 衰减状态（挡板消失后）
|
||||
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
|
||||
**方向固定为右（+1.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)
|
||||
---
|
||||
|
||||
## 中线变形
|
||||
|
||||
当 `lidar_is_active()`（确认中或保持衰减中）时，对 row 10 ~ 障碍行施加变形：
|
||||
|
||||
```
|
||||
decay = (确认中) 1.0 : (1.0 - hold_ctr / hold_frames) // 保持期线性衰减
|
||||
|
||||
for row = 10 to g_lidar_hold_src_row:
|
||||
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)
|
||||
```
|
||||
|
||||
---
|
||||
@@ -244,100 +142,46 @@ else if g_lidar_hold_src_row > 0 && g_lidar_hold_ctr < lidar_hold_frames:
|
||||
▲ │
|
||||
└────────────────────────────────┘ 测距恢复正常
|
||||
|
||||
NORMAL: 正常行驶，激光测距 > THRESHOLD
|
||||
AVOID: 挡板确认 → 中线变形绕行 + 减速 × lidar_speed
|
||||
NORMAL: 正常行驶，激光测距 > lidar_pre
|
||||
AVOID: 挡板确认 → 中线变形绕行 (固定朝右)
|
||||
HOLD_DECAY: 挡板消失 → 变形量线性衰减 (lidar_hold_frames 帧内归零)
|
||||
衰减期间若再次检测到挡板 → 立即切回 AVOID
|
||||
```
|
||||
|
||||
| 状态 | 电机 | 舵机 | 说明 |
|
||||
|------|------|------|------|
|
||||
| NORMAL | 正常速度 | 正常巡线 | 无变形 |
|
||||
| AVOID | speed × lidar_speed | 中线偏转绕行 | 基于 left/right 空间判断方向 |
|
||||
| HOLD_DECAY | speed × lidar_speed | 变形量线性衰减 | 确保车完全通过后再恢复 |
|
||||
|
||||
---
|
||||
|
||||
## 与现有流水线的集成
|
||||
## 速度联动
|
||||
|
||||
`lidar_avoid_process()` 放在 cone_detect_and_deform 之前执行（两者都修改 mid_line，
|
||||
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()`
|
||||
`motor_update()` 中包含挡板减速代码（`lidar_is_active()` → `speed × lidar_speed`），
|
||||
但当前**已注释**。绕行仅靠中线变形，不减速。如需启用，取消注释即可。
|
||||
|
||||
---
|
||||
|
||||
## 配置参数
|
||||
|
||||
| 文件 | 类型 | 建议默认 | 含义 |
|
||||
|------|------|----------|------|
|
||||
| `./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_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_avoid_gain` | double | 0.4 | 绕行中线推离力度 (归一化) |
|
||||
| `./lidar_avoid_range` | int | 30 | 绕行斜坡陡峭度 (行数) |
|
||||
| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 |
|
||||
| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 (当前未生效) |
|
||||
| `./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，不阻塞正常行驶。
|
||||
2. **弯道边墙误判** — 急弯处赛道边墙可能导致激光近距 + 边线丢失同时出现。依赖 `lidar_thresh` 和 `lidar_min_frames` 调参抑制。误判时车会短暂绕向一侧，弯道通过后立即恢复。
|
||||
3. **左右空间相等** — `left_space == right_space` 时默认 `dir = -1.0`（往右绕），可通过配置 `lidar_default_dir` 调整。
|
||||
4. **上坡/下坡** — 车辆俯仰变化会影响距离→行的映射精度。建议在平坦路段标定。
|
||||
5. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。
|
||||
6. **首次集成建议** — 先调通数据采集：打印 `d_mm`、对应 `row`、以及 `near_ref_row` 处的 `left/right/mid` 值，跑几圈确认标定参数无误后再开启绕行。
|
||||
7. **挡板材质** — 深色挡板不会被 HSV-Otsu 判为赛道（白色 255），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是**丢线**而非**像素颜色**。
|
||||
1. **激光硬件未连接** — `init()` 失败时 `g_lidar_ok = false`，后续跳过，不阻塞。
|
||||
2. **弯道边墙误判** — 急弯处激光可能打到边墙产生近距读数。依赖 `lidar_thresh` + `lidar_min_frames` 去抖抑制。
|
||||
3. **方向固定右绕** — 当前不判断挡板在赛道左侧还是右侧，一律朝右推中线。对大多数场景够用，但若挡板在右侧则绕行方向不理想。
|
||||
4. **标定建议** — 在平坦路段用 debug 模式打印 `d_mm` 和 `row`，确认 `lidar_near` / `lidar_far` 映射准确。
|
||||
5. **VL53L0X 读取模式** — 非阻塞：先 `startMeasure()` 启动后台测量 (~20ms)，下次调用时 `readResult()` 取结果。每 2 帧一读不阻塞主循环。
|
||||
6. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。
|
||||
|
||||
+14
-12
@@ -2,11 +2,11 @@
|
||||
|
||||
## 1. 模型检测
|
||||
|
||||
NanoDet V10 模型 4 类输出中：
|
||||
Mild v12 模型 4 类输出中：
|
||||
- cls=1: 红灯
|
||||
- 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. 状态机
|
||||
|
||||
@@ -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 || 等待绿灯
|
||||
```
|
||||
@@ -67,20 +68,21 @@ LCD 状态: "R"=红灯停车 "G"=等绿灯, 拼接斑马线状态
|
||||
## 5. 关键代码路径
|
||||
|
||||
```
|
||||
CameraHandler (src/camera.cpp)
|
||||
Step 4: 模型推理 → g_boxes[], g_box_count
|
||||
Step 5: 斑马线状态机
|
||||
Step 5.5: 红绿灯状态机
|
||||
CameraHandler (src/camera.cpp:755)
|
||||
Step 5: 模型推理 → g_boxes[], g_box_count
|
||||
Step 6a: 斑马线状态机
|
||||
Step 6b: 红绿灯状态机
|
||||
├─ 遍历 g_boxes, 统计 red_seen/green_seen
|
||||
├─ 去抖计数 (3帧确认, 衰减-1/帧)
|
||||
└─ 状态跃迁
|
||||
Step 5.6: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行)
|
||||
Step 7: 电机控制
|
||||
ControlUpdate(spd, g_zstate==Z_STOP || tl_block)
|
||||
Step 6c: 锥桶检测 (g_zstate!=Z_STOP && g_tl_state==TL_NORMAL 时才运行)
|
||||
Step 8: 电机控制
|
||||
motor_update(zebra_block, tl_block)
|
||||
→ ControlUpdate(final_spd, zebra_block || tl_block)
|
||||
|
||||
control.cpp:
|
||||
ControlUpdate(speed, zebra_block)
|
||||
└─ zebra_block → 编码器线程读速度 → 比例刹车
|
||||
ControlUpdate(speed, block)
|
||||
└─ block → 编码器线程读速度 → 比例刹车
|
||||
```
|
||||
|
||||
## 6. 状态变量
|
||||
|
||||
+10
-2
@@ -12,6 +12,8 @@ const std::string start_file = "./start";
|
||||
const std::string showImg_file = "./showImg";
|
||||
const std::string destfps_file = "./destfps";
|
||||
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 saveImg_file = "./saveImg";
|
||||
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_thresh_file = "./cone_thresh";
|
||||
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_max_file = "./brake_max";
|
||||
@@ -56,6 +60,8 @@ void cfg_load_all();
|
||||
struct CfgCache {
|
||||
double speed = 60; // 目标速度(占空比 %)
|
||||
double foresee = 80; // 前瞻行
|
||||
double foresee_lost_scale = 0.7; // 丢线时前瞻缩放 (<1=看更远)
|
||||
double sharp_turn_scale = 0.5; // 急弯时前瞻缩放 (<1=看更远)
|
||||
double zebrasee = 60; // 斑马线触发距离阈值
|
||||
double deadband = 5; // 舵机死区
|
||||
double steer_gain = 1.0; // 舵机增益
|
||||
@@ -67,10 +73,12 @@ struct CfgCache {
|
||||
double cone_avoid_gain = 0.3; // 锥桶中线变形推离量 (归一化)
|
||||
int cone_avoid_range = 30; // 变形斜坡陡峭度
|
||||
double cone_speed = 0.5; // 锥桶触发时速度倍率
|
||||
int cone_min_frames = 3; // 连续确认帧数
|
||||
int cone_min_frames = 1; // 确认帧数 (1=单帧即确认, 视觉巡线响应快导致检测窗口极短)
|
||||
int cone_margin = 0; // 0=中心点 1=框边缘
|
||||
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) 缩放系数
|
||||
int brake_max = 10000; // 最大刹车占空比 (ns)
|
||||
|
||||
@@ -12,5 +12,7 @@ extern cv::Mat track;
|
||||
extern std::vector<int> left_line;
|
||||
extern std::vector<int> right_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;
|
||||
|
||||
@@ -4,3 +4,6 @@ target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS})
|
||||
|
||||
add_executable(lidar_test lidar_test.cpp)
|
||||
target_link_libraries(lidar_test common_lib)
|
||||
|
||||
add_executable(remote_control remote_control.cpp)
|
||||
target_link_libraries(remote_control common_lib)
|
||||
@@ -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;
|
||||
}
|
||||
+523
-157
File diff suppressed because it is too large
Load Diff
+70
-16
@@ -1,8 +1,15 @@
|
||||
/*
|
||||
* control — 电机控制层
|
||||
*
|
||||
* 开环占空比控制 + 斑马线编码器比例刹车。
|
||||
* 开环占空比控制 + 编码器比例刹车（斑马线/红绿灯停车时触发）。
|
||||
* 编码器线程: 高速轮询 gpio67 → 算速度(脉冲/秒) → 原子变量共享。
|
||||
*
|
||||
* 硬件映射:
|
||||
* motor[0] = 左电机 pwmchip8/pwm2, GPIO12 方向
|
||||
* motor[1] = 右电机 pwmchip8/pwm1, GPIO13 方向
|
||||
* GPIO73 = 电机使能 (mortorEN)
|
||||
* GPIO67 = 编码器 LSB 脉冲 (输入)
|
||||
* GPIO72 = 编码器方向 (输入)
|
||||
*/
|
||||
#include "control.h"
|
||||
#include "global.h"
|
||||
@@ -10,17 +17,27 @@
|
||||
#include <atomic>
|
||||
#include <cmath>
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 全局 & 静态变量
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
MotorController *motorController[2] = {nullptr, nullptr};
|
||||
GPIO mortorEN(73);
|
||||
GPIO mortorEN(73); // 电机使能引脚
|
||||
|
||||
static GPIO encoderLSB(67);
|
||||
static GPIO encoderDIR(72);
|
||||
static GPIO encoderLSB(67); // 编码器 LSB 脉冲
|
||||
static GPIO encoderDIR(72); // 编码器方向
|
||||
|
||||
static std::thread g_enc_thread;
|
||||
static std::atomic<bool> g_enc_running{false};
|
||||
static std::atomic<double> g_enc_speed{0.0};
|
||||
static std::atomic<int> g_enc_dir{0};
|
||||
static std::atomic<double> g_enc_speed{0.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() {
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
@@ -36,43 +53,58 @@ static void encoder_thread() {
|
||||
int cur_lsb = encoderLSB.readValue() ? 1 : 0;
|
||||
int cur_dir = encoderDIR.readValue() ? 1 : 0;
|
||||
|
||||
// 检测 LSB 边沿变化 → 计脉冲
|
||||
if (cur_lsb != prev_lsb) {
|
||||
pulse_cnt++;
|
||||
prev_lsb = cur_lsb;
|
||||
}
|
||||
g_enc_dir.store(cur_dir);
|
||||
|
||||
// 每 100ms 窗口输出一次速度
|
||||
long t1 = enc_now_ns();
|
||||
long dt = t1 - t0;
|
||||
if (dt >= 100000000L) {
|
||||
if (dt >= 100000000L) { // 100ms
|
||||
g_enc_speed.store(pulse_cnt / (dt / 1e9));
|
||||
pulse_cnt = 0;
|
||||
t0 = t1;
|
||||
}
|
||||
|
||||
// 500µs 间隔，防止 CPU 饿死其他线程
|
||||
std::this_thread::sleep_for(std::chrono::microseconds(500));
|
||||
}
|
||||
}
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// ControlInit — 初始化电机 + 编码器线程
|
||||
//
|
||||
// 调用时机: main.cpp 启动时，CameraInit 之后。
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
void ControlInit()
|
||||
{
|
||||
// 电机使能引脚
|
||||
mortorEN.setDirection("out");
|
||||
mortorEN.setValue(1);
|
||||
|
||||
// 编码器引脚
|
||||
encoderLSB.setDirection("in");
|
||||
encoderDIR.setDirection("in");
|
||||
|
||||
// 启动编码器测速线程
|
||||
g_enc_running.store(true);
|
||||
g_enc_thread = std::thread(encoder_thread);
|
||||
|
||||
// GPIO13 (左电机 IN2) 设为高电平，配合 PWM 实现 H 桥正/反转
|
||||
GPIO leftIn2(13);
|
||||
leftIn2.setDirection("out");
|
||||
leftIn2.setValue(1);
|
||||
|
||||
// 创建双电机控制器
|
||||
// motor[0]: pwmchip8/pwm2 + GPIO12 方向 (左电机)
|
||||
// motor[1]: pwmchip8/pwm1 + GPIO13 方向 (右电机)
|
||||
const int pwmchip[2] = {8, 8};
|
||||
const int pwmnum[2] = {2, 1};
|
||||
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)
|
||||
{
|
||||
@@ -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)
|
||||
{
|
||||
// ── 模式 1: 未启动 → 停转 ──
|
||||
if (!g_cfg.start)
|
||||
{
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i]) motorController[i]->updateduty(0);
|
||||
mortorEN.setValue(0);
|
||||
mortorEN.setValue(0); // 拉低使能
|
||||
return;
|
||||
}
|
||||
|
||||
// ── 模式 2: 刹车 (斑马线/红绿灯停车) ──
|
||||
if (zebra_block)
|
||||
{
|
||||
double cur_speed = g_enc_speed.load();
|
||||
int cur_dir = g_enc_dir.load();
|
||||
double cur_speed = g_enc_speed.load(); // 原子读: 编码器速度 (pps)
|
||||
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);
|
||||
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;
|
||||
|
||||
// 施加反向 PWM: 方向取反实现刹车
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i])
|
||||
motorController[i]->updateduty(dir ? -brake_pct : brake_pct);
|
||||
}
|
||||
else
|
||||
else // 已停止 (< 0.5 pps)
|
||||
{
|
||||
// 不再施加刹车，防止反冲
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i]) motorController[i]->updateduty(0);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// ── 模式 3: 正常行驶 → 开环占空比 ──
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i]) motorController[i]->updateduty(speed);
|
||||
|
||||
mortorEN.setValue(1);
|
||||
mortorEN.setValue(1); // 使能拉高
|
||||
}
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// ControlExit — 停止编码器线程 + 释放电机资源
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
void ControlExit()
|
||||
{
|
||||
// 停止编码器线程
|
||||
g_enc_running.store(false);
|
||||
if (g_enc_thread.joinable()) g_enc_thread.join();
|
||||
|
||||
// 释放电机控制器
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
delete motorController[i];
|
||||
motorController[i] = nullptr;
|
||||
}
|
||||
mortorEN.setValue(0);
|
||||
mortorEN.setValue(0); // 使能拉低
|
||||
}
|
||||
|
||||
@@ -24,6 +24,8 @@ void cfg_load_all()
|
||||
{
|
||||
g_cfg.speed = readDoubleFromFile(speed_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.deadband = readDoubleFromFile(deadband_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_range = (int)readDoubleFromFile(cone_avoid_range_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_max = readDoubleFromFile(brake_max_file);
|
||||
|
||||
+10
-2
@@ -30,6 +30,9 @@ cv::Mat track;
|
||||
std::vector<int> left_line;
|
||||
std::vector<int> right_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_height; // 处理高度 = 60
|
||||
@@ -105,7 +108,8 @@ cv::Mat find_road(cv::Mat &frame)
|
||||
cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);
|
||||
|
||||
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. 种子点位置
|
||||
// X = 图像水平中心 (line_tracking_width/2)
|
||||
@@ -130,7 +134,8 @@ cv::Mat find_road(cv::Mat &frame)
|
||||
nullptr, loDiff, upDiff, 8);
|
||||
|
||||
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);
|
||||
|
||||
return outputImage;
|
||||
@@ -244,11 +249,13 @@ void image_main()
|
||||
|
||||
// ── 6. 中线计算 + 丢线补全 ─────
|
||||
// 从倒数第二行开始向上，底行(row=height-1)单独兜底避免 mid_line[row+1] 越界
|
||||
g_lost_rows = 0;
|
||||
for (int row = line_tracking_height - 2; row >= 10; --row)
|
||||
{
|
||||
// ── 6a. 丢线补全 ──────────────────────────────
|
||||
if (left_line[row] == -1 && right_line[row] == -1)
|
||||
{
|
||||
g_lost_rows++;
|
||||
// 当前行完全丢线: 用下行(row+1)的中线来虚拟补线
|
||||
mid_line[row] = mid_line[row + 1];
|
||||
|
||||
@@ -278,6 +285,7 @@ void image_main()
|
||||
const int row = line_tracking_height - 1;
|
||||
if (left_line[row] == -1 && right_line[row] == -1)
|
||||
{
|
||||
g_lost_rows++;
|
||||
mid_line[row] = line_tracking_width / 2;
|
||||
left_line[row] = 0;
|
||||
right_line[row] = line_tracking_width - 1;
|
||||
|
||||
Reference in New Issue
Block a user