spdis
123 lines
4.0 KiB
Markdown
123 lines
4.0 KiB
Markdown
# 编码器刹车 — 设计与实现
|
||
|
||
## 1. 硬件
|
||
|
||
编码器引脚：
|
||
|
||
| 信号 | 引脚 | 模式 |
|
||
|---|---|---|
|
||
| LSB 脉冲 | GPIO 67 | 输入 |
|
||
| DIR 方向 | GPIO 72 | 输入 |
|
||
|
||
两引脚均属 gpiochip64（GPA64，base=64，16 pins）。
|
||
|
||
## 2. 编码器线程
|
||
|
||
`ControlInit()` 启动独立线程 `encoder_thread()`（`src/control.cpp:30`），持续运行直到 `ControlExit()`。
|
||
|
||
```
|
||
encoder_thread():
|
||
高频轮询 GPIO67 (LSB 边沿) + GPIO72 (方向)
|
||
轮询间隔: 500µs sleep (~2kHz)
|
||
|
||
每 100ms 窗口:
|
||
g_enc_speed = pulse_cnt / elapsed_time (脉冲/秒, 原子变量)
|
||
g_enc_dir = GPIO72 当前值 (原子变量)
|
||
pulse_cnt 归零, 开始新窗口
|
||
```
|
||
|
||
通过 sysfs 文件描述符轮询（`GPIO::readValue()`），`lseek` + `read`。不依赖中断。
|
||
|
||
## 3. 触发条件
|
||
|
||
`ControlUpdate(speed, block)` 中 `block == true` 时进入刹车流程。
|
||
|
||
`block` 信号来源（两者取 OR）：
|
||
- **斑马线**: `zebra_process()` 返回 true 当 `g_zstate == Z_STOP`
|
||
- **红绿灯**: `traffic_light_process()` 返回 true 当 `g_tl_state != TL_NORMAL`
|
||
|
||
调用链: `motor_update(zebra_block, tl_block)` → `ControlUpdate(final_spd, zebra_block || tl_block)`
|
||
|
||
## 4. 比例刹车
|
||
|
||
无状态机，每帧根据编码器实时速度计算刹车力度：
|
||
|
||
```
|
||
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)
|
||
```
|
||
|
||
**特点：**
|
||
- **比例刹车**: 速度越快 → 刹车占空比越大 → 刹车力越强
|
||
- **自动停止**: 速度降到 0.5 pps 以下自动释放刹车，防止过冲
|
||
- **反向驱动**: 根据编码器方向 `cur_dir` 施加反方向 PWM
|
||
|
||
## 5. 参数
|
||
|
||
可通过文本文件热更新（debug 模式下每 30 帧重载）：
|
||
|
||
| 文件 | 类型 | 默认 | 含义 |
|
||
|---|---|---|---|
|
||
| `./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. 关键代码路径
|
||
|
||
```
|
||
main.cpp
|
||
└── while(running)
|
||
└── CameraHandler()
|
||
├── 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
|
||
```
|