05｜速度环与位置环：滤波、前馈 PID 和目标规划

电机驱动和编码器组成了“输出—反馈”通道，控制算法负责闭合这条链。当前工程采用左右独立速度内环，并可在外层叠加位置控制、循迹修正和目标速度规划。

1. 为什么先有速度内环

同样的 PWM 在不同电量、负载和地面条件下不会得到固定转速。速度环比较目标 RPM 与编码器 RPM，并实时修正输出，使上层可以用“希望车轮多快”而不是“猜一个占空比”描述任务。

目标 RPM ──┐
           ├→ 误差 → PID + 前馈 → PWM → 电机
实际 RPM ──┘                         ↑
      └──────── 编码器反馈 ─────────┘

左右轮分别保存滤波、积分、历史误差和输出状态。这样机械差异不会被一个平均控制器掩盖。

SpeedPID_Update() 对实际 RPM 使用一阶低通：

滤波值 += α ×（原始值 - 滤波值）

当前 α 在代码中配置为 0.35。较小的 α 更平滑但延迟更大，较大的 α 响应更快但保留更多波动。它是当前软件参数，不等于所有车辆和周期下的最佳值。

滤波放在控制模块而不是编码器驱动中，有两个好处：原始测量仍可诊断，控制器也能明确管理滤波初始化和复位。

控制输出由目标前馈和误差反馈共同组成：

输出 = Kf × 目标
     + Kp × 误差
     + Ki × 积分
     + Kd × 测量变化率

前馈先给出与目标速度成比例的基础输出，反馈再修正负载与模型误差。代码中的 D 项对测量变化率求导，避免目标突然变化时产生典型的微分冲击。

当前实现包含多项工程保护：

这些逻辑不是为了让公式更复杂，而是为了避免停车后残留积分、换向冲击和长期饱和。

SpeedTarget_ApplyRamp() 不允许目标 RPM 一步跳到最终值。它按控制周期限制每次变化量，加速和减速可使用不同速率；正反切换时先把目标拉回零。

应用目标 → 限幅 → 判断加速/减速/换向 → 每周期最大步进 → PID 目标

斜坡限制的是“控制器收到的目标”，并不保证车轮一定按同样加速度运动。真实响应仍取决于电源、负载、地面和速度环参数。

PositionControl_Update() 计算目标位置与当前距离的误差，再把位置误差转换为左右目标 RPM。远离目标时使用比例关系并限制最大速度；进入精调区域后使用较低速度；进入容差后目标归零。

目标距离 - 当前距离
          ↓
      位置外环
          ↓
   左右目标 RPM
          ↓
   左右速度内环

位置环不直接输出 PWM。它只给速度环设目标，从而保留速度闭环对负载变化的补偿能力。

当前应用支持不同的位置模式：

模式是应用策略，不应写入编码器驱动或 TB6612 驱动。底层始终只提供左右测量和左右输出。

PositionControl_IsAtTarget() 除了检查位置误差，还要求左右实际 RPM 都进入停止范围。普通减速流程也要求目标斜坡与实际速度连续多个周期满足停止条件，之后才清理状态并禁止电机。

这避免车辆因为惯性刚越过距离阈值，就被错误判定为稳定完成。任务完成应同时包含：

位置条件成立
+ 目标已降到零附近
+ 左右实际速度足够小
+ 连续满足稳定周期

如果速度内环尚未稳定就调位置环，外层误差会掩盖内层问题，参数也难以迁移。

明确唯一写入者可以减少状态互相覆盖。新应用模式应生成目标 RPM，而不是绕过 Task_PID() 直接写电机。