“Arduino BLDC 之机器人阿基米德螺旋扫描 + 纯追踪 + FOC 速度闭环”可以理解为：上层按阿基米德螺旋方程 r = a + b·θ 连续生成参考轨迹，中层用纯追踪算法根据机器人当前位姿和前视点计算期望线速度与角速度，底层由 BLDC FOC 速度环执行左右轮速度指令，从而实现由内向外或由外向内的高精度螺旋覆盖运动。
一、主要特点

1. 阿基米德螺旋轨迹天然适合全覆盖扫描
   阿基米德螺旋的极坐标形式为 r = a + b·θ，相邻圈间距恒定，适合清洁、巡检、测绘、喷涂、探测等需要均匀覆盖区域的场景。与简单圆弧或随机行走相比，它路径连续、转向平滑、覆盖率可控。
2. 轨迹生成采用参数化递推
   不需要预先存储大量航点，只需按时间或角度增量递推生成当前参考点：θ(k) = θ₀ + k·Δθ，r(k) = a + b·θ(k)，再转换为直角坐标 x = r·cosθ，y = r·sinθ。这样计算量小，适合 Arduino 实时运行。
3. 纯追踪算法将轨迹误差转化为速度指令
   纯追踪在参考轨迹上选取一个前视点，根据机器人当前位置与前视点的几何关系计算转弯半径和期望角速度。前视距离过短容易振荡，过长则跟踪滞后，需要根据螺旋曲率动态调整。
4. FOC 速度闭环保证低速平滑执行
   螺旋轨迹在靠近中心处曲率大、速度低，传统方波驱动容易出现抖动和爬行。FOC 通过电流环解耦转矩与磁场，配合速度 PID 和低通滤波，可在低速、频繁转向时保持低转矩脉动。
5. 差速底盘实现左右轮速分解
   纯追踪输出的线速度 v 和角速度 ω 需分解为左右轮目标速度：v_L = v - ω·L/2，v_R = v + ω·L/2，其中 L 为轮距。FOC 速度环分别跟踪左右轮速度，形成“轨迹 → 纯追踪 → 差速分解 → FOC 速度环”的完整链路。
6. 位姿估计是闭环基础
   纯追踪需要知道机器人当前 (x, y, θ)。低成本方案可用编码器航位推算 + IMU 航向校正；更高精度可加入视觉里程计或外部定位。位姿漂移会直接导致螺旋轨迹偏离。
   二、应用场景
7. 清洁机器人全覆盖路径
   在房间或指定区域内由中心向外螺旋清扫，保证地面覆盖均匀，减少重复路径和漏扫区域。
8. 室内巡检与气体探测
   机器人从某一点开始螺旋扩展扫描，用于检测温湿度、气体浓度、噪声或辐射分布，形成空间热力图。
9. 地面喷涂/标记/测绘
   在平整地面上按螺旋轨迹喷涂、划线或采集地面图像，适合需要等间距覆盖的作业。
10. 农业温室/园区巡检
    在开放区域中以螺旋方式扫描作物、土壤或设施状态，适合小范围精细化探测。
11. 教育与算法验证平台
    用于验证参数化轨迹生成、纯追踪前视距离整定、差速运动学、FOC 速度环调参和位姿估计。
12. 仓储/展厅引导演示
    机器人以螺旋方式展开表演或引导路径，视觉效果明显，适合展示路径跟踪能力。
    三、需要注意的事项
13. 螺旋参数要与底盘能力匹配
    螺距 b 过小会导致转弯过密，差速底盘无法精确执行；b 过大会降低覆盖率。应先根据最小转弯半径和轮距确定可用曲率范围。
14. 前视距离应随曲率自适应
    螺旋越靠近中心，曲率越大，前视距离应缩短；外圈曲率小，可适当增大前视距离。固定前视距离容易造成内圈振荡或外圈滞后。
15. 线速度和角速度必须限幅
    螺旋中心附近角速度可能急剧增大，若不限制会导致原地打转、轮端打滑或电流尖峰。应设置 v_max、ω_max 和加速度上限。
16. 位姿漂移会累积
    仅靠编码器积分，长时间螺旋运动后 (x, y) 会明显偏离真实位置。建议加入 IMU 航向校正，或在关键节点用外部参考点修正。
17. FOC 速度环需先单独调稳
    在接入轨迹跟踪前，应先让左右轮速度环能稳定跟踪阶跃和斜坡速度指令。速度环不稳定时，纯追踪误差会被放大为机身抖动。
18. 控制周期要分层
    FOC 电流环和速度环应运行在固定高频中断中，纯追踪和轨迹生成可运行在较低频率主循环中。串口打印、日志写入不应阻塞电机控制。
19. 螺旋起点和方向需明确
    可选择由内向外或由外向内扫描。由内向外时，起点定位误差影响较小；由外向内时，终点附近曲率极大，更容易失稳。
20. 打滑和地面摩擦会影响轨迹
    螺旋运动依赖差速转向，若地面过滑或负载不均，实际轨迹会偏离理论螺旋。可通过编码器速度闭环部分补偿，但无法完全消除滑移误差。
21. Arduino 算力需评估
    ATmega328P 可承担单圈螺旋、简单纯追踪和双路 FOC 速度环；若同时运行 IMU 融合、日志记录、复杂滤波或多传感器融合，建议迁移到 ESP32-S3、Teensy 4.1、STM32 或采用分层架构。
22. 应设置覆盖完成与异常退出条件
    例如达到最大半径、覆盖率达到阈值、连续跟踪误差超限、电机过流或通信超时。避免机器人无限螺旋或偏离区域后无法恢复。

整体链路是：设定螺旋参数 a、b 和扫描方向 → 按 θ 递推生成参考点 → 估计机器人当前位姿 → 纯追踪选取前视点并计算 v、ω → 差速分解为左右轮目标速度 → FOC 速度环跟踪执行 → 监控跟踪误差、打滑和覆盖进度 → 达到终止条件后退出或切换任务。做原型时，建议按“FOC 单轮速度环 → 双轮差速闭环 → 固定圆弧跟踪 → 纯追踪直线/圆弧 → 阿基米德螺旋轨迹生成 → 自适应前视距离 → 位姿校正与限幅保护”逐步推进。

1、阿基米德螺旋生成 + 纯追踪前视点跟踪 + FOC 速度环（核心框架）
适用场景：机器人在开阔区域执行覆盖式扫描（如地面清洁、环境探测），沿阿基米德螺旋线从中心向外扩展，螺旋间距略小于传感器有效幅宽以保证覆盖率。纯追踪算法沿螺旋路径计算前视点，输出线速度与角速度，交由 FOC 速度环执行。

#include <SimpleFOC.h>

// 双轮 BLDC 差速底盘
BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9, 10, 11, 8), drvR(22, 23, 24, 25);
Encoder encL(2, 3, 2048), encR(32, 33, 2048);

// ===== 车体与螺旋参数（直接来自搜索结果） =====
const float R_W = 0.065f;        // 轮半径 m
const float TRACK = 0.34f;       // 轮距 m
const float V_SCAN = 0.28f;      // 扫描线速度 m/s
const float PITCH  = 0.26f;      // 螺旋间距 m（≈传感器幅宽×0.85）
const float LOOKAHEAD = 0.22f;   // 前视距离 m

// ===== 位姿与里程 =====
float px = 0, py = 0, pth = 0;   // 世界系位姿
float vCur = 0, wCur = 0;

// ===== 螺旋参数 =====
float thetaSpiral = 0;            // 当前螺旋参数角

// 由 theta 求笛卡尔点
void spiralPoint(float th, float &x, float &y) {
  float r = PITCH * th / (2.0f * PI);
  x = r * cosf(th);
  y = r * sinf(th);
}

// 沿螺旋前进 ds 弧长对应的 dθ
float thetaStep(float th, float ds) {
  float r = PITCH * th / (2.0f * PI);
  float drdth = PITCH / (2.0f * PI);
  float jac = sqrtf(r*r + drdth*drdth);
  return ds / max(jac, 0.02f);
}

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
  drvL.voltage_power_supply = 24; drvR.voltage_power_supply = 24;
  drvL.init(); drvR.init();
  wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);
  
  // FOC 速度闭环
  wheelL.controller = wheelR.controller = MotionControlType::velocity;
  wheelL.PID_velocity.P = 0.35f; wheelL.PID_velocity.I = 4.0f;
  wheelR.PID_velocity.P = 0.35f; wheelR.PID_velocity.I = 4.0f;
  
  wheelL.init(); wheelL.initFOC();
  wheelR.init(); wheelR.initFOC();
}

void loop() {
  wheelL.loopFOC(); wheelR.loopFOC();

  // ===== 1. 沿螺旋推进一个步长 =====
  float ds = V_SCAN * 0.01f;  // 10ms 周期的弧长增量
  thetaSpiral += thetaStep(thetaSpiral, ds);

  // ===== 2. 当前前视点（螺旋上的点） =====
  float lookX, lookY;
  spiralPoint(thetaSpiral + LOOKAHEAD / max(PITCH * thetaSpiral / (2*PI), 0.05f), lookX, lookY);

  // ===== 3. 纯追踪：计算到前视点的方向 =====
  float dx = lookX - px;
  float dy = lookY - py;
  float distToLook = sqrtf(dx*dx + dy*dy);
  float targetHeading = atan2f(dy, dx);
  
  // 航向误差
  float dth = targetHeading - pth;
  while (dth > PI) dth -= 2*PI;
  while (dth < -PI) dth += 2*PI;

  // ===== 4. 差速控制量 =====
  vCur = V_SCAN;
  wCur = 2.0f * V_SCAN * sinf(dth) / max(distToLook, 0.05f);  // 纯追踪曲率公式

  // 左右轮目标速度 (m/s)
  float vL = vCur - wCur * TRACK / 2.0f;
  float vR = vCur + wCur * TRACK / 2.0f;

  // 转为电机角速度 (rad/s) 交给 FOC
  wheelL.move(vL / R_W);
  wheelR.move(vR / R_W);

  // ===== 5. 里程计更新 =====
  px += vCur * cosf(pth) * 0.01f;
  py += vCur * sinf(pth) * 0.01f;
  pth += wCur * 0.01f;

  delay(10);
}

解读：该案例直接复用了搜索结果中的螺旋参数与纯追踪结构。纯追踪的核心是 wCur = 2Vsin(dth)/distToLook，它根据前视点的航向偏差和距离计算曲率，使机器人沿弧线趋近前视点而非直线切向。利用 SimpleFOC 的闭环跟踪确保左右轮实际转速与指令一致。

2、螺旋参数自适应 + 纯追踪前视距离动态调整
场景：固定 PITCH 和 LOOKAHEAD 在螺旋半径增大时会出现跟踪误差累积——外圈曲率变小，固定前视距离导致转向“迟钝”；内圈曲率大，固定前视距离又过于激进。本案例根据当前螺旋半径动态调整前视距离，并在螺旋间距上引入传感器幅宽反馈，实现自适应覆盖。

#include <SimpleFOC.h>

BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9, 10, 11, 8), drvR(22, 23, 24, 25);
Encoder encL(2, 3, 2048), encR(32, 33, 2048);

// 车体参数
const float R_W = 0.065f;
const float TRACK = 0.34f;
const float V_SCAN = 0.25f;

// 螺旋参数（可自适应）
float PITCH_BASE = 0.26f;
float pitchCurrent = 0.26f;
float lookaheadBase = 0.22f;

float px = 0, py = 0, pth = 0;
float thetaSpiral = 0;

void spiralPoint(float th, float &x, float &y) {
  float r = pitchCurrent * th / (2.0f * PI);
  x = r * cosf(th);
  y = r * sinf(th);
}

float thetaStep(float th, float ds) {
  float r = pitchCurrent * th / (2.0f * PI);
  float drdth = pitchCurrent / (2.0f * PI);
  float jac = sqrtf(r*r + drdth*drdth);
  return ds / max(jac, 0.02f);
}

// ===== 动态前视距离：随螺旋半径增大而增大 =====
float adaptiveLookahead(float theta) {
  float r = pitchCurrent * theta / (2.0f * PI);
  // 内圈（r<0.5）用小前视，外圈（r>1.5）用大前视
  float scale = constrain(r / 1.0f, 0.6f, 1.8f);
  return lookaheadBase * scale;
}

// ===== 螺旋间距自适应：根据传感器反馈调整 =====
void updatePitch(float sensorWidth) {
  // 间距设为传感器幅宽的 85%，留 15% 重叠
  pitchCurrent = sensorWidth * 0.85f;
  pitchCurrent = constrain(pitchCurrent, 0.15f, 0.40f);
}

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
  drvL.voltage_power_supply = 24; drvR.voltage_power_supply = 24;
  drvL.init(); drvR.init();
  wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);
  wheelL.controller = wheelR.controller = MotionControlType::velocity;
  wheelL.PID_velocity.P = 0.35f; wheelL.PID_velocity.I = 4.0f;
  wheelR.PID_velocity.P = 0.35f; wheelR.PID_velocity.I = 4.0f;
  wheelL.init(); wheelL.initFOC();
  wheelR.init(); wheelR.initFOC();
  
  // 初始间距（模拟传感器幅宽 0.30m）
  updatePitch(0.30f);
}

void loop() {
  wheelL.loopFOC(); wheelR.loopFOC();

  float ds = V_SCAN * 0.01f;
  thetaSpiral += thetaStep(thetaSpiral, ds);

  // 动态前视距离
  float lookDist = adaptiveLookahead(thetaSpiral);
  float lookTheta = thetaSpiral + lookDist / max(pitchCurrent * thetaSpiral / (2*PI), 0.05f);

  float lookX, lookY;
  spiralPoint(lookTheta, lookX, lookY);

  float dx = lookX - px;
  float dy = lookY - py;
  float distToLook = sqrtf(dx*dx + dy*dy);
  float targetHeading = atan2f(dy, dx);
  float dth = targetHeading - pth;
  while (dth > PI) dth -= 2*PI;
  while (dth < -PI) dth += 2*PI;

  float vCur = V_SCAN;
  float wCur = 2.0f * vCur * sinf(dth) / max(distToLook, 0.05f);

  float vL = vCur - wCur * TRACK / 2.0f;
  float vR = vCur + wCur * TRACK / 2.0f;

  wheelL.move(vL / R_W);
  wheelR.move(vR / R_W);

  px += vCur * cosf(pth) * 0.01f;
  py += vCur * sinf(pth) * 0.01f;
  pth += wCur * 0.01f;

  delay(10);
}

解读：该案例在案例一基础上增加了两个自适应机制。adaptiveLookahead() 根据当前螺旋半径 rr 缩放前视距离：内圈曲率大，短前视使转向更灵敏；外圈曲率小，长前视避免频繁修正造成的蛇形。updatePitch() 根据传感器实际幅宽调整螺旋间距，保证相邻两圈有 15% 重叠，避免漏扫。搜索结果中螺旋参数讨论提到“螺旋参数 NN 在覆盖率与精细度之间提供权衡”，pitchCurrent 的自适应正是对这一权衡的工程化实现。

3、螺旋扫描 + 纯追踪 + 避障局部修正（扫描不中断）
场景：扫描过程中前方出现临时障碍物。纯追踪沿螺旋路径的指令被避障逻辑临时旁路，但螺旋参数 θθ 继续推进，避障结束后机器人从“当前 θθ 对应位置”附近重新接入螺旋，而非从障碍前的位置重走。这借鉴了搜索结果中螺旋避障“动态调整螺旋参数绕过障碍后恢复”的思想。

#include <SimpleFOC.h>
#include <NewPing.h>

BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9, 10, 11, 8), drvR(22, 23, 24, 25);
Encoder encL(2, 3, 2048), encR(32, 33, 2048);

NewPing sonarF(4, 5, 200);
NewPing sonarL(6, 7, 200);
NewPing sonarR(8, 9, 200);

const float R_W = 0.065f;
const float TRACK = 0.34f;
const float V_SCAN = 0.25f;
const float PITCH = 0.26f;
const float LOOKAHEAD = 0.22f;
const float OBSTACLE_SAFE = 0.25f;  // 25cm 避障阈值

float px = 0, py = 0, pth = 0;
float thetaSpiral = 0;

enum Mode { SPIRAL, AVOID };
Mode mode = SPIRAL;
unsigned long avoidStart = 0;

void spiralPoint(float th, float &x, float &y) {
  float r = PITCH * th / (2.0f * PI);
  x = r * cosf(th);
  y = r * sinf(th);
}

float thetaStep(float th, float ds) {
  float r = PITCH * th / (2.0f * PI);
  float drdth = PITCH / (2.0f * PI);
  float jac = sqrtf(r*r + drdth*drdth);
  return ds / max(jac, 0.02f);
}

void setup() {
  Serial.begin(115200);
  encL.init(); encR.init();
  wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
  drvL.voltage_power_supply = 24; drvR.voltage_power_supply = 24;
  drvL.init(); drvR.init();
  wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);
  wheelL.controller = wheelR.controller = MotionControlType::velocity;
  wheelL.PID_velocity.P = 0.35f; wheelL.PID_velocity.I = 4.0f;
  wheelR.PID_velocity.P = 0.35f; wheelR.PID_velocity.I = 4.0f;
  wheelL.init(); wheelL.initFOC();
  wheelR.init(); wheelR.initFOC();
}

void loop() {
  wheelL.loopFOC(); wheelR.loopFOC();

  float dF = sonarF.ping_cm() / 100.0f;
  float dL = sonarL.ping_cm() / 100.0f;
  float dR = sonarR.ping_cm() / 100.0f;

  // ===== 状态仲裁 =====
  bool obstacle = (dF > 0 && dF < OBSTACLE_SAFE);
  if (obstacle && mode == SPIRAL) {
    mode = AVOID;
    avoidStart = millis();
    Serial.println("→ 避障");
  } else if (mode == AVOID && !obstacle && millis() - avoidStart > 600) {
    mode = SPIRAL;
    Serial.println("→ 恢复螺旋");
  }

  // ===== 螺旋参数持续推进（避障期间也推进，但速度降半） =====
  float ds = V_SCAN * (mode == SPIRAL ? 0.01f : 0.005f);
  thetaSpiral += thetaStep(thetaSpiral, ds);

  float vL, vR;

  if (mode == AVOID) {
    // 硬避障：后退转向
    if (dL > dR) {
      vL = -0.15f; vR = 0.20f;
    } else {
      vL = 0.20f; vR = -0.15f;
    }
  } else {
    // ===== 纯追踪 =====
    float lookTheta = thetaSpiral + LOOKAHEAD / max(PITCH * thetaSpiral / (2*PI), 0.05f);
    float lookX, lookY;
    spiralPoint(lookTheta, lookX, lookY);

    float dx = lookX - px;
    float dy = lookY - py;
    float distToLook = sqrtf(dx*dx + dy*dy);
    float targetHeading = atan2f(dy, dx);
    float dth = targetHeading - pth;
    while (dth > PI) dth -= 2*PI;
    while (dth < -PI) dth += 2*PI;

    float vCur = V_SCAN;
    float wCur = 2.0f * vCur * sinf(dth) / max(distToLook, 0.05f);

    vL = vCur - wCur * TRACK / 2.0f;
    vR = vCur + wCur * TRACK / 2.0f;
  }

  wheelL.move(vL / R_W);
  wheelR.move(vR / R_W);

  // 里程计更新（避障期间用实际速度近似）
  float vAvg = (vL + vR) / 2.0f;
  float wAvg = (vR - vL) / TRACK;
  px += vAvg * cosf(pth) * 0.01f;
  py += vAvg * sinf(pth) * 0.01f;
  pth += wAvg * 0.01f;

  delay(10);
}

解读：该案例的关键设计是 thetaSpiral 在避障期间继续推进（速度降半）。如果避障时冻结螺旋参数，恢复后机器人需要从障碍前的位置重新走到障碍点，浪费已扫描区域。持续推进使避障结束后机器人“跳过”被障碍占据的螺旋段，从障碍另一侧继续扫描，牺牲一小段覆盖率换取扫描连续性。本案例的 thetaSpiral 推进逻辑与之等价——用参数推进代替空间位置的严格跟踪。

要点解读

1. 阿基米德螺旋的“间距”参数直接决定扫描覆盖率与效率的权衡
   搜索结果中 PITCH = 0.26f 的注释是“≈传感器幅宽×0.85”，这意味着相邻两圈有 15% 重叠。如果 PITCH 等于或大于传感器幅宽，圈与圈之间会出现漏扫带；如果远小于幅宽，扫描路径长度增加但覆盖率提升有限。螺旋参数讨论也指出“螺旋参数 NN 主要影响覆盖密度而非效率”。案例二的 updatePitch() 将这一参数与传感器实际幅宽绑定，是工程上合理的自适应方式。

2. 纯追踪的核心是“前视点”而非“当前路径点”，前视距离决定跟踪的平滑度与精度
   案例一的 LOOKAHEAD = 0.22f 使机器人朝螺旋上“前方 22cm 处的点”运动，而非严格贴着当前螺旋点。前视距离过短（如 5cm）会导致机器人频繁修正航向，产生蛇形振荡；前视距离过长（如 50cm）会在内圈曲率大的区域“切弯”，偏离螺旋路径。案例二的 adaptiveLookahead() 根据半径缩放前视距离，是解决这一矛盾的合理方案。

3. FOC 速度闭环是纯追踪指令被精确执行的前提
   如果底层使用开环 ESC 或有刷电机 PWM，低速段的死区和负载变化会导致实际轮速偏离指令，机器人的实际轨迹与螺旋路径产生累积偏差。SimpleFOC 的速度环通过编码器反馈实时修正电压/电流，使 wheelL.move(vL/R_W) 的角速度指令被跟踪。搜索结果中该案例的 FOC 速度环参数（P=0.35, I=4.0）是经过整定的参考值。

4. 避障期间螺旋参数应继续推进，而非冻结
   案例三的 thetaSpiral 在 AVOID 模式下以半速推进。如果冻结参数，避障结束后机器人回到螺旋上的位置与障碍物位置相同，纯追踪会再次引导机器人撞向障碍物。参数推进使机器人“跳过”障碍段，从障碍另一侧继续。搜索结果中螺旋避障策略的核心也是“动态调整螺旋参数绕过障碍，离开后恢复”。代价是被跳过的螺旋段未被扫描，需要在后续补扫或接受覆盖率的轻微损失。

5. Arduino 算力下，螺旋生成与纯追踪应降频，FOC 环保持高频
   案例中的 delay(10) 对应 100Hz 的螺旋推进与纯追踪计算频率。螺旋点生成涉及 sinf/cosf/sqrtf，纯追踪涉及 atan2f/sinf，在 Arduino Uno 上以 100Hz 运行已接近负载。合理的架构是：FOC 环以最大频率运行（去掉 delay 或使用定时器），螺旋推进与纯追踪降频到 50-100Hz。搜索结果中该案例推荐 ESP32 而非 Uno，正是因为 Uno 的 8 位架构和有限 SRAM 难以同时承载双电机 FOC 和浮点路径计算。如果必须使用 Uno，应将螺旋点的三角函数用查表法预计算，减少实时浮点运算。

4、螺旋参数化 + 纯追踪 + FOC 速度闭环（最小闭环）
适用硬件：ESP32 + 差速 BLDC（2×SimpleFOC）+ 编码器。

核心思路：阿基米德螺旋 r = B·θ 是纯几何，真正难的是“机器人如何始终贴着这条曲线走”。做法是：每周期把机器人当前位置投影回螺旋得到当前 θ，再沿螺旋前视一段找到目标点，用纯追踪算出应有的曲率，转成左右轮速差，交给 FOC 速度环执行。

/*
 * 案例4：阿基米德螺旋 + 纯追踪 + FOC 速度闭环
 */
#include <SimpleFOC.h>

BLDCMotor wl(7), wr(7);
BLDCDriver3PWM dl(9,10,11,8), dr(22,23,24,25);
Encoder el(2,3,2048), er(32,33,2048);
void onL(){ el.handleA(); }
void onR(){ er.handleA(); }

const float R_W   = 0.065f;
const float TRACK = 0.34f;

// ---------------- 螺旋几何 ----------------
const float PITCH = 0.26f;                  // 相邻圈间距（覆盖约束，非随意设定）
const float B     = PITCH / (2.0f * PI);    // r = B·θ
const float R_MIN = 0.12f;                  // 起始半径：避开中心曲率奇异
const float R_MAX = 2.40f;                  // 扫描外径

// ---------------- 跟踪参数 ----------------
const float V_SCAN = 0.28f;
const float LOOK   = 0.22f;        // 前视距离
const float A_LAT  = 0.70f;        // 侧向加速度上限 m/s²

// ---------------- 状态 ----------------
float theta = R_MIN / B;           // 当前螺旋参数
float px = 0, py = 0, pth = 0;     // 机器人位姿
bool  done = false;

void spiralPt(float th, float &x, float &y) {
  float r = B * th;
  x = r * cosf(th);
  y = r * sinf(th);
}

// ---------------- 位姿投影回螺旋 ----------------
// 在当前 theta 附近搜索最近点，避免全局搜索
float projectOntoSpiral() {
  float bestTh = theta, bestD2 = 1e9f;
  for (int k = -12; k <= 12; k++) {
    float th = theta + k * 0.03f;
    if (th < R_MIN/B) continue;
    float x, y; spiralPt(th, &x, &y);
    float dx = x - px, dy = y - py;
    float d2 = dx*dx + dy*dy;
    if (d2 < bestD2) { bestD2 = d2; bestTh = th; }
  }
  return bestTh;
}

// ---------------- 前视点：沿弧长推进 ----------------
bool findLookahead(float th0, float &tx, float &ty, float &thOut) {
  float th = th0;
  for (int i = 0; i < 500; i++) {
    float x, y; spiralPt(th, &x, &y);
    float d = hypotf(x - px, y - py);
    if (d >= LOOK) { tx = x; ty = y; thOut = th; return true; }

    float r = B * th;
    // 弧长微分：ds = sqrt(r² + B²)·dθ  =>  dθ = ds / sqrt(r²+B²)
    th += 0.02f / sqrtf(r*r + B*B);        // 每次前进 2 cm 弧长
    if (th * B > R_MAX) break;
  }
  return false;
}

// ---------------- 控制输出 ----------------
float vCmd = 0, wCmd = 0;

void setup() {
  el.init(); el.enableInterrupts(onL);
  er.init(); er.enableInterrupts(onR);
  wl.linkSensor(&el); wr.linkSensor(&er);

  // ---- FOC 速度环三件套 ----
  wl.controller = wr.controller = MotionControlType::velocity;
  wl.PID_velocity.P = 0.35f;  wl.PID_velocity.I = 4.0f;
  wr.PID_velocity.P = 0.35f;  wr.PID_velocity.I = 4.0f;
  wl.PID_velocity.output_ramp = 400.0f;      // 抑制指令阶跃
  wr.PID_velocity.output_ramp = 400.0f;
  wl.LPF_velocity.Tf = 0.01f;                // 滤掉编码器量化噪声
  wr.LPF_velocity.Tf = 0.01f;
  wl.voltage_limit = wr.voltage_limit = 12.0f;
  wl.current_limit = wr.current_limit = 4.0f;  // 力矩上限=最后保险

  wl.init(); wr.init();
  dl.voltage_power_supply = dr.voltage_power_supply = 12.0f;
  dl.init(); dr.init();
  wl.linkDriver(&dl); wr.linkDriver(&dr);
  wl.initFOC(); wr.initFOC();

  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev) / 1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.2f) dt = 0.02f;

  if (!done) {
    // ---- 1. 投影 -> 前视点 ----
    theta = projectOntoSpiral();
    float tx, ty, thTgt;
    if (!findLookahead(theta, tx, ty, thTgt)) {
      done = true;                        // 螺旋走完
    } else {
      // ---- 2. 纯追踪：kappa = 2·sin(alpha) / L ----
      float dx = tx - px, dy = ty - py;
      float L  = hypotf(dx, dy);
      float alpha = atan2f(dy, dx) - pth;
      while (alpha >  PI) alpha -= 2*PI;
      while (alpha < -PI) alpha += 2*PI;

      float kappa = 2.0f * sinf(alpha) / max(L, 0.05f);

      // ---- 3. 曲率限速：v <= sqrt(a_lat / |kappa|) ----
      float vLim = (fabsf(kappa) > 1e-3f)
                 ? sqrtf(A_LAT / fabsf(kappa)) : V_SCAN;
      float vTgt = min(V_SCAN, vLim);
      vTgt = min(vTgt, 0.9f * L / 0.22f * V_SCAN);   // 目标点太近也减速

      // ---- 4. 曲率 -> 角速度 ----
      float wTgt = vTgt * kappa;

      // ---- 5. 速率限幅（平滑）----
      vCmd += constrain(vTgt - vCmd, -1.2f*dt, 0.8f*dt);
      wCmd += constrain(wTgt - wCmd, -3.0f*dt, 3.0f*dt);
      wCmd = constrain(wCmd, -1.6f, 1.6f);
    }
  } else {
    vCmd += constrain(0 - vCmd, -1.2f*dt, 0);   // 结束：平滑停车
    wCmd += constrain(0 - wCmd, -3.0f*dt, 3.0f*dt);
  }

  // ---- 6. 差速分解 -> FOC 执行 ----
  float vL = vCmd - wCmd * TRACK * 0.5f;
  float vR = vCmd + wCmd * TRACK * 0.5f;
  wl.loopFOC(); wr.loopFOC();
  wl.move(vL / R_W);
  wr.move(vR / R_W);

  // ---- 7. 里程 ----
  float vl = wl.shaft_velocity * R_W;
  float vr = wr.shaft_velocity * R_W;
  float v = 0.5f * (vl + vr), w = (vr - vl) / TRACK;
  px  += v * cosf(pth) * dt;
  py  += v * sinf(pth) * dt;
  pth += w * dt;

  Serial.print("r="); Serial.print(B*theta, 2);
  Serial.print(" v="); Serial.print(vCmd, 2);
  Serial.print(" w="); Serial.print(wCmd, 2);
  Serial.print(" done="); Serial.println(done);
  delay(20);
}

关键点：projectOntoSpiral 每次只在当前 θ 附近搜 ±12 个点（约 ±0.36 rad），而不是全局搜。因为螺旋是连续曲线、机器人不会跳变，局部搜索既快又不会“跳到相邻圈上去”——这一步做错，机器人会在两圈之间来回跳，表现为原地打摆。

案例5：三层分离 + 螺旋间距自适应 + 曲率前馈 + 位置闭环纠偏
适用硬件：ESP32 双核 + 差速 BLDC + 编码器 + 侧向测距（用于覆盖宽度估计）。

核心思路：案例一是纯开环几何——里程一漂，螺旋就偏心。这一版三件事：① 间距由传感器有效幅宽反算，而不是写死；② 加速度/曲率前馈加进去，纯追踪只修残差；③ 三层任务分离，路径规划 50 Hz、纠偏 100 Hz、执行 1 kHz，且用横向误差做位置闭环。

/*
 * 案例5：间距自适应 + 曲率前馈 + 横向误差闭环
 */
#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor wl(7), wr(7);
// 驱动/编码器初始化同案例一（略）
SemaphoreHandle_t mux;

const float R_W = 0.065f, TRACK = 0.34f;

// ---- 螺旋参数（PITCH 可变）----
float B = 0.26f / (2.0f*PI);
const float R_MIN = 0.12f, R_MAX = 2.40f;
const float V_SCAN = 0.30f, A_LAT = 0.70f;

// ---- 共享 ----
volatile float g_v = 0, g_w = 0;
volatile bool  g_done = false;
float px = 0, py = 0, pth = 0;     // 由执行层更新
float theta = R_MIN / (0.26f/(2*PI));

// ---- 传感器有效幅宽：左右两侧测距之和 + 机体宽度 ----
const int TRIG_L = 6, ECHO_L = 7, TRIG_R = 8, ECHO_R = 9;
float readUS(int t, int e) {
  digitalWrite(t, LOW); delayMicroseconds(2);
  digitalWrite(t, HIGH); delayMicroseconds(10); digitalWrite(t, LOW);
  long us = pulseIn(e, HIGH, 15000);
  if (us <= 0) return 9.0f;
  return us * 0.0343f / 2.0f / 100.0f;
}

// 覆盖率驱动的间距：幅宽 × 0.85（留 15% 重叠，防漏扫）
void updatePitch() {
  float wL = readUS(TRIG_L, ECHO_L);
  float wR = readUS(TRIG_R, ECHO_R);
  float span = constrain(wL + wR + 0.34f, 0.20f, 1.20f);
  float pitch = 0.85f * span;
  B = pitch / (2.0f*PI);
}

void spiralPt(float th, float &x, float &y) {
  float r = B*th; x = r*cosf(th); y = r*sinf(th);
}

// 解析曲率：kappa = (r² + 2B²) / (r² + B²)^1.5
float spiralCurv(float th) {
  float r = B*th;
  float d = r*r + B*B;
  return (r*r + 2*B*B) / (d * sqrtf(d));
}

float projectOntoSpiral() {
  float bestTh = theta, bestD2 = 1e9f;
  for (int k = -12; k <= 12; k++) {
    float th = theta + k*0.03f;
    if (th < R_MIN/B) continue;
    float x,y; spiralPt(th,&x,&y);
    float dx=x-px, dy=y-py, d2=dx*dx+dy*dy;
    if (d2 < bestD2) { bestD2=d2; bestTh=th; }
  }
  return bestTh;
}

// ---------------- 任务1：间距自适应 5Hz ----------------
void taskPitch(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) { updatePitch(); vTaskDelayUntil(&last, pdMS_TO_TICKS(200)); }
}

// ---------------- 任务2：路径规划 50Hz ----------------
void taskPlan(void*) {
  TickType_t last = xTaskGetTickCount();
  float vCmd = 0, wCmd = 0;

  for (;;) {
    theta = projectOntoSpiral();

    // 前视点
    float th = theta, tx = 0, ty = 0, found = 0;
    for (int i = 0; i < 400; i++) {
      float x,y; spiralPt(th,&x,&y);
      if (hypotf(x-px, y-py) >= 0.22f) { tx=x; ty=y; found=1; break; }
      float r = B*th;
      th += 0.02f / sqrtf(r*r + B*B);
      if (r > R_MAX) break;
    }

    if (!found) {
      g_done = true;
      vCmd += constrain(0 - vCmd, -0.05f, 0);
      wCmd += constrain(0 - wCmd, -0.15f, 0.15f);
    } else {
      float dx = tx-px, dy = ty-py;
      float L = hypotf(dx,dy);
      float alpha = atan2f(dy,dx) - pth;
      while (alpha > PI) alpha -= 2*PI;
      while (alpha < -PI) alpha += 2*PI;

      // 纯追踪曲率 + 螺旋解析曲率（前馈）加权
      float kPP  = 2.0f*sinf(alpha) / max(L,0.05f);
      float kFF  = spiralCurv(th);
      float kappa = 0.65f*kFF + 0.35f*kPP;

      float vLim = (fabsf(kappa) > 1e-3f)
                 ? sqrtf(A_LAT/fabsf(kappa)) : V_SCAN;
      float vTgt = min(V_SCAN, vLim);
      float wTgt = vTgt * kappa;

      vCmd += constrain(vTgt-vCmd, -1.2f*0.02f, 0.8f*0.02f);
      wCmd += constrain(wTgt-wCmd, -3.0f*0.02f, 3.0f*0.02f);
      wCmd = constrain(wCmd, -1.6f, 1.6f);
    }

    xSemaphoreTake(mux, portMAX_DELAY);
    g_v = vCmd; g_w = wCmd;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(20));
  }
}

// ---------------- 任务3：执行 1kHz ----------------
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    float v, w;
    xSemaphoreTake(mux, portMAX_DELAY);
    v = g_v; w = g_w;
    xSemaphoreGive(mux);

    float vL = v - w*TRACK*0.5f, vR = v + w*TRACK*0.5f;
    wl.loopFOC(); wr.loopFOC();
    wl.move(vL/R_W); wr.move(vR/R_W);

    float vl = wl.shaft_velocity*R_W, vr = wr.shaft_velocity*R_W;
    px += 0.5f*(vl+vr)*cosf(pth)*0.001f;
    py += 0.5f*(vl+vr)*sinf(pth)*0.001f;
    pth += (vr-vl)/TRACK*0.001f;
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
  pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);
  // 电机初始化同案例一（含 LPF / output_ramp / current_limit）
  theta = R_MIN / B;
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskPitch, "pitch", 4096, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(taskPlan,  "plan",  4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExec,  "exec",  4096, NULL, 3, NULL, 1);
  Serial.begin(115200);
}
void loop() { vTaskDelay(portMAX_DELAY); }

关键点：曲率用了 0.65·kFF + 0.35·kPP 的加权，而不是纯纯追踪。原因是螺旋的解析曲率是已知的（spiralCurv），前馈进去可以让纯追踪只负责修正偏差——这相当于给控制器“先喂一个大概正确的前馈量”，残差小、超调也小。纯追踪单独用时，弯道总要滞后一点才追上来。

案例6：扫描任务管理 —— 覆盖完成判定 + 遇障挂起与弧长恢复
适用硬件：ESP32 + 差速 BLDC + 编码器 + 前向超声（障碍） + 侧向测距。

核心思路：真实场地一定有障碍。螺旋扫描不能“遇到障碍就停”或“绕过去继续按 θ 走”（那样会漏掉一片）。正确做法是用弧长参数 s 记录进度：遇障时把 s 存下来，绕行结束后回到 s 继续；同时用覆盖半径判定任务是否完成（r ≥ R_MAX 或覆盖率达标），而不是“转够多少圈”。

/*
 * 案例6：扫描任务管理 + 遇障挂起 + 弧长恢复
 */
#include <SimpleFOC.h>

BLDCMotor wl(7), wr(7);
// 初始化同案例一（略）

const float R_W = 0.065f, TRACK = 0.34f;
const float PITCH = 0.26f, B = PITCH/(2.0f*PI);
const float R_MIN = 0.12f, R_MAX = 2.40f;
const float V_SCAN = 0.28f, A_LAT = 0.70f, LOOK = 0.22f;

// ---- 任务状态 ----
enum ScanState { S_SCAN, S_SUSPEND, S_DETOUR, S_RESUME, S_DONE };
ScanState st = S_SCAN;

// ---- 弧长进度（关键：用 s 而不是 theta 记进度）----
float sDone = 0;            // 已完成弧长
float sSuspend = 0;         // 挂起时刻的弧长
float detourT0 = 0;

// ---- 位姿 ----
float px = 0, py = 0, pth = 0;
float vCmd = 0, wCmd = 0;

// ---- 障碍 ----
const int TRIG_F = 10, ECHO_F = 11;
const int TRIG_L = 6,  ECHO_L = 7;
const int TRIG_R = 8,  ECHO_R = 9;

float readUS(int t, int e) {
  digitalWrite(t, LOW); delayMicroseconds(2);
  digitalWrite(t, HIGH); delayMicroseconds(10); digitalWrite(t, LOW);
  long us = pulseIn(e, HIGH, 15000);
  if (us <= 0) return 9.0f;
  return us * 0.0343f / 2.0f / 100.0f;
}

// ---- 由弧长反解 theta：s(θ) ≈ ∫sqrt(r²+B²)dθ，用增量累加 ----
float thetaOfArc(float sTarget) {
  float th = R_MIN/B, s = 0;
  while (s < sTarget) {
    float r = B*th;
    float dth = 0.01f / sqrtf(r*r + B*B);
    th += dth;
    s += 0.01f;
    if (th*B > R_MAX) break;
  }
  return th;
}

void spiralPt(float th, float &x, float &y) {
  float r = B*th; x = r*cosf(th); y = r*sinf(th);
}

float projectOntoSpiral(float thNear) {
  float bestTh = thNear, bestD2 = 1e9f;
  for (int k = -12; k <= 12; k++) {
    float th = thNear + k*0.03f;
    if (th < R_MIN/B) continue;
    float x,y; spiralPt(th,&x,&y);
    float dx=x-px, dy=y-py, d2=dx*dx+dy*dy;
    if (d2 < bestD2) { bestD2=d2; bestTh=th; }
  }
  return bestTh;
}

float thetaCur = R_MIN/B;

void setup() {
  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
  pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
  pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);
  // 电机初始化同案例一
  Serial.begin(115200);
}

void loop() {
  static unsigned long tPrev = millis();
  unsigned long now = millis();
  float dt = (now - tPrev)/1000.0f; tPrev = now;
  if (dt <= 0 || dt > 0.2f) dt = 0.02f;

  float dF = readUS(TRIG_F, ECHO_F);
  float dL = readUS(TRIG_L, ECHO_L);
  float dR = readUS(TRIG_R, ECHO_R);

  float vTgt = 0, wTgt = 0;

  switch (st) {
    // ---------------- 正常扫描 ----------------
    case S_SCAN: {
      thetaCur = projectOntoSpiral(thetaCur);

      // 前视
      float th = thetaCur, tx=0, ty=0; bool found = false;
      for (int i = 0; i < 400; i++) {
        float x,y; spiralPt(th,&x,&y);
        if (hypotf(x-px,y-py) >= LOOK) { tx=x; ty=y; found=true; break; }
        float r = B*th;
        th += 0.02f / sqrtf(r*r + B*B);
        if (r > R_MAX) break;
      }

      if (!found) { st = S_DONE; break; }

      float dx = tx-px, dy = ty-py;
      float L = hypotf(dx,dy);
      float alpha = atan2f(dy,dx) - pth;
      while (alpha > PI) alpha -= 2*PI;
      while (alpha < -PI) alpha += 2*PI;

      float kappa = 2.0f*sinf(alpha)/max(L,0.05f);
      float vLim = (fabsf(kappa)>1e-3f) ? sqrtf(A_LAT/fabsf(kappa)) : V_SCAN;
      vTgt = min(V_SCAN, vLim);
      wTgt = vTgt * kappa;

      // 障碍：挂起，记录弧长
      if (dF < 0.40f) {
        sSuspend = sDone;
        st = S_SUSPEND;
        detourT0 = millis();
        Serial.print("[SCAN] suspend at s="); Serial.println(sSuspend, 2);
      }
      break;
    }

    // ---------------- 挂起：减速停车 ----------------
    case S_SUSPEND: {
      vTgt = 0; wTgt = 0;
      if (fabsf(vCmd) < 0.02f) { st = S_DETOUR; detourT0 = millis(); }
      break;
    }

    // ---------------- 绕行：沿空侧偏出 ----------------
    case S_DETOUR: {
      float side = (dL > dR) ? 1.0f : -1.0f;
      vTgt = 0.22f;
      wTgt = side * 0.6f;

      // 前方通畅 且 已走 0.5s -> 准备回到螺旋
      if (dF > 0.55f && millis()-detourT0 > 500) {
        st = S_RESUME;
        detourT0 = millis();
      }
      else if (millis()-detourT0 > 6000) { st = S_RESUME; detourT0 = millis(); }
      break;
    }

    // ---------------- 恢复：回到挂起点前 0.15m 的螺旋上 ----------------
    case S_RESUME: {
      // 回到 sSuspend 对应螺旋点
      float thBack = thetaOfArc(max(0.0f, sSuspend - 0.15f));
      float tx, ty; spiralPt(thBack, &tx, &ty);

      float dx = tx-px, dy = ty-py;
      float d = hypotf(dx,dy);

      if (d < 0.25f) {
        thetaCur = thBack;
        st = S_SCAN;
        Serial.println("[SCAN] resumed");
      } else {
        float alpha = atan2f(dy,dx) - pth;
        while (alpha > PI) alpha -= 2*PI;
        while (alpha < -PI) alpha += 2*PI;
        vTgt = constrain(0.8f*min(d,0.8f), 0.0f, 0.45f);
        wTgt = constrain(2.0f*alpha, -0.9f, 0.9f);
        if (fabsf(alpha) > 1.2f) vTgt = 0;
      }
      // 恢复也超时：放弃返回，直接重规划（此处简化为继续扫描）
      if (millis()-detourT0 > 5000) { thetaCur = thetaOfArc(sSuspend); st = S_SCAN; }
      break;
    }

    case S_DONE:
      vTgt = 0; wTgt = 0;
      break;
  }

  // ---- 平滑 + FOC 执行 ----
  vCmd += constrain(vTgt - vCmd, -1.2f*dt, 0.8f*dt);
  wCmd += constrain(wTgt - wCmd, -3.0f*dt, 3.0f*dt);
  wCmd = constrain(wCmd, -1.6f, 1.6f);

  float vL = vCmd - wCmd*TRACK*0.5f;
  float vR = vCmd + wCmd*TRACK*0.5f;
  wl.loopFOC(); wr.loopFOC();
  wl.move(vL/R_W); wr.move(vR/R_W);

  // ---- 里程 + 弧长累加 ----
  float vl = wl.shaft_velocity*R_W, vr = wr.shaft_velocity*R_W;
  float v = 0.5f*(vl+vr), w = (vr-vl)/TRACK;
  px += v*cosf(pth)*dt; py += v*sinf(pth)*dt; pth += w*dt;
  if (st == S_SCAN) sDone += fabsf(v)*dt;

  Serial.print("st="); Serial.print((int)st);
  Serial.print(" s="); Serial.print(sDone, 1);
  Serial.print(" r="); Serial.println(B*thetaCur, 2);
  delay(20);
}

关键点：恢复时回到的是 sSuspend - 0.15f，即挂起点之前一小段，而不是挂起点本身。因为绕行期间机器人已经偏离，直接瞄准挂起点会在螺旋上“切一刀”，留下一小块漏扫；退后 15 cm 重新进线，保证覆盖连续。

要点解读
一、螺旋的间距由覆盖幅宽决定，不是绘图时觉得好看就行
PITCH = 0.85 × 有效幅宽 是覆盖率约束的直接体现：间距大于幅宽必然漏扫，间距过小则重复率过高、效率下降。案例二把这一步做成了在线自适应——用左右测距之和反算当前通道的有效幅宽，再动态改 B。这个改动看起来小，但在实际场地（通道宽窄不一）里直接决定“扫得全不全”，比任何跟踪算法的调参都更影响最终结果。

二、中心曲率发散，必须设最小起始半径
阿基米德螺旋在原点处的曲率趋于 2/B（间距 0.26 m 时约为 48 /m，曲率半径仅 2 cm）。这个曲率是任何差速底盘都跟不了的——内侧轮要反转、外侧轮要极速，实际表现是原地打滑、里程狂漂。所以 R_MIN = 0.12 不是保守，是物理必需。同理，靠近中心时速度必须降下来（见第三点），否则一次打滑就把整条螺旋的相位搞乱。

三、前视距离是“稳定性 vs 滞后”的唯一调节旋钮
纯追踪里 LOOK 太大 → 机器人抄近道、切掉弯道内侧（螺旋会变成越来越偏心的圆）；LOOK 太小 → 目标点总在脚下，α 剧烈抖动，车头来回摆。经验值是与车体长度同量级（0.2~0.3 m），并且前视点必须沿曲线搜索，不能沿直线插值——沿直线找“距离 0.22 m 的点”在弯道上会落到曲线外面去。

四、螺旋上的速度不是常数，必须由曲率反算
v ≤ sqrt(a_lat / |κ|) 是这条曲线的物理速度包络：外圈 κ 小、可以快；内圈 κ 大、必须慢。案例一每周期重算 vLim，案例二更进一步用解析曲率 (r²+2B²)/(r²+B²)^1.5 做前馈。反例是“恒定速度走螺旋”——外圈浪费时间，内圈必然打滑，最终结果是内外圈都不准。

五、FOC 速度环是“执行保真”的前提，光有规划不够
路径算得再准，如果轮速跟不上指令，螺旋照样变形。速度环的三件套缺一不可：LPF_velocity 滤编码器量化噪声（否则微分项狂抖）、output_ramp 抑制指令阶跃（保护机械、避免电流冲击）、current_limit 作为最后的力矩保险（打滑时不会烧管）。再加上带宽分层：速度环 1 kHz、路径环 20~50 Hz，两者差一个数量级以上，路径环的延迟才不会污染速度环。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。