模型推理移至管道末尾降舵机延迟60→16ms + 视觉Mat复用 + 端到端延迟计时 + OpenCV 4.13 + WASD遥控
This commit is contained in:
spdis
+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
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user