# 锥桶避障功能 — 中线变形方案

## 1. 核心思路

**不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。

不使用任何推入计时器、归还逻辑、EMA 平滑——锥桶消失后，下一帧 `image_main()` 重新计算 `mid_line`，自动恢复。

---

## 2. 坐标与边界过滤

```
模型空间 (raw_frame):
  正常模式: 160×120 (MJPEG 1/4解码)
  回退模式: 640×480

巡线空间映射:
  cone_col = box.cx * line_tracking_width  / raw_frame.cols   (典型 0~79)
  cone_row = box.cy * line_tracking_height / raw_frame.rows   (典型 0~59)

换算速查 (正常模式 160×120 → 典型 80×60):
  cone_col = cx / 2
  cone_row = cy / 2
```

边界过滤 (cls=0, conf ≥ cone_thresh):
```
row_lt < 10                             → 丢弃 (row 0~9 无可靠巡线数据)
row_lt >= line_tracking_height           → 丢弃
left_line[row_lt] == -1                 → 丢弃 (该行丢线)
right_line[row_lt] == -1                → 丢弃
col_lt < left_line[row_lt]              → 丢弃 (赛道外)
col_lt > right_line[row_lt]             → 丢弃 (赛道外)
```

---

## 3. 中线变形

### 3.1 方向

```
mid_at_cone = mid_line[cone_row]

cone_col < mid_at_cone  →  锥在左 → push_mid 向右 (+)
cone_col > mid_at_cone  →  锥在右 → push_mid 向左 (-)

direction = (cone_col < mid_at_cone) ? +1 : -1
```

### 3.2 变形区域

从 `start_row = max(10, cone_row - range)` 到 `cone_row`，共 (cone_row - start_row) 行：

```
row 0 ───────────────────────────────── 远
     │  (不变形)
row 10 ─────────────────────────────────
     │  start_row ────────────── t=0
     │  线性斜坡
     │  foresee 行在这里看到部分偏移
     │
     │  cone_row ────────────── t=1
row 59 ───────────────────────────────── 近 (车前方)
```

### 3.3 数学

```
for row in [start_row, cone_row]:
    t = (row - start_row) / (cone_row - start_row)    // [0, 1]

    push = t * cone_avoid_gain * (line_tracking_width / 2) * direction

    mid_line[row] = original_mid_line[row] + push
    mid_line[row] = clamp(mid_line[row],
                          left_line[row] + 2,
                          right_line[row] - 2)
```

**参数：**
- `cone_avoid_gain` (double, 0.3)：归一化推离量。1.0 = 推到半幅赛道宽。默认 0.3 表示推到赛道 30% 宽度处。
- `cone_avoid_range` (int, 30)：斜坡覆盖行数。必须足够大使 foresee 行感受到偏移。

---

## 4. 数字算例

**场景：** 直道，锥桶在赛道左侧。`lt_w=80, lt_h=60, cone_avoid_gain=0.3, cone_avoid_range=30`

```
原始数据:
  cone_col=25, cone_row=50, mid_at_cone=40 (锥在左侧15列)
  direction = +1 (推中线向右)
  start_row = max(10, 50-30) = 20
  half_w = 40

变形计算:
  push_max = 0.3 * 40 * 1 = 12 (行50处的最大位移, 巡线空间列)

  row=20 (start, t=0):     mid_line = 40 + 0     = 40  → deviation = 40*2-80 = 0    → 舵机中位
  row=25 (t=5/30=0.17):    mid_line = 40 + 2.04  = 42  → deviation = 42*2-80 = +4   → 略右
  row=35 (t=15/30=0.5):    mid_line = 40 + 6.0   = 46  → deviation = 46*2-80 = +12  → 明显右转
  row=50 (t=1.0):          mid_line = 40 + 12    = 52  → deviation = 52*2-80 = +24  → 大幅右转

foresee=40 (显示空间) → check_row=40/2=20 (巡线空间):
  mid_line[20] 未被变形 (start_row=20, t=0)
  → 舵机看不到! 需要减小 range 或增大 foresee
```

**问题暴露：** `foresee=40` → `check_row=20`，但 `start_row=20` 时 t=0 无偏移。

**修正：** 确保变形起点在 `check_row` 以上。两种方式：
1. 增大 `cone_avoid_range`（如 40），让 start_row 更低
2. 让斜坡向上延伸超过 start_row（即从 row=10 就开始）

**采用方案 2：** 变形始终从 `row=10` 开始，`range` 参数控制斜坡的"陡峭度"而非范围。

---

## 5. 修正后的变形公式

```
range = cone_avoid_range                      // 陡峭度参数 (默认 30)
total_rows = cone_row - 10                    // 实际跨度

for row in [10, cone_row]:
    t_raw = (row - 10.0) / total_rows         // 线性 [0, 1]
    t = clamp(t_raw * (total_rows / range),   // 用 range 缩放斜率
              0.0, 1.0)

    push = t * cone_avoid_gain * (line_tracking_width / 2) * direction
    mid_line[row] = clamp(original_mid + push,
                          left_line[row] + 2,
                          right_line[row] - 2)
```

**效果（cone_avoid_range=30, cone_row=50, total_rows=40）：**

```
total_rows/range = 40/30 = 1.33  ← 斜率因子 >1, 斜坡更陡

row=10:  t_raw=0,    t=0*1.33=0       → push=0
row=20:  t_raw=0.25, t=0.25*1.33=0.33 → push=0.33*12=4.0 → deviation=+8  (刚好过死区)
row=35:  t_raw=0.625,t=0.83           → push=10      → deviation=+20
row=50:  t_raw=1.0,  t=1.0            → push=12      → deviation=+24
```

foresee 行 (row=20) 收到 33% 的偏移，产生 +8px 偏差 → 刚好越过 deadband(8px) → 舵机开始右转。

**调参规则：**
- 增大 `cone_avoid_range` → 斜坡更缓 → foresee 行偏移更小 → 转向更柔和
- 减小 `cone_avoid_range` → 斜坡更陡 → foresee 行偏移更大 → 转向更猛
- 增大 `cone_avoid_gain` → 整体偏移量增大 → 转向更猛

---

## 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)
```

**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。
- 锥桶在 → 变形在（每帧重新计算 mid_line 变形）
- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失
- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑

---

## 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;
}
ControlUpdate(effective_speed, g_zstate == Z_STOP);
```

---

## 8. 参数配置

### 8.1 启动时一次性读取（运行中不重新读文件）

| 文件 | 类型 | 默认 | 含义 |
|---|---|---|---|
| `./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_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
```

---

## 9. 代码修改清单

```
lib/global.h:
  ├── CfgCache 新增: cone_avoid_gain, cone_avoid_range,
  │                   cone_speed, cone_min_frames, cone_margin, cone_thresh
  └── 新增文件名常量

src/global.cpp:
  └── cfg_load_all() 追加热更新参数 (不含 avoid_gain/avoid_range)

main/main.cpp:
  └── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg

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: 锥桶框改橙色

ctl.sh:
  └── init_pins() 追加 6 个新文件默认值
```

---

## 10. 方案对比

| | 旧方案（舵机推入） | 新方案（中线变形） |
|---|---|---|
| 舵机代码 | 需改动 | 零改动 |
| deadband | 需绕过 | 自 然生效 |
| steer_gain | 需单独 cone_gain | 共用 |
| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) |
| 平滑性 | 需 EMA | 斜坡自带平滑 |
| 参数数量 | 7 | 6 |
| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 |