“Arduino BLDC 之仓库 AGV 机器人 - S 曲线规划器 + 航点队列管理”通常指：用 S 型速度曲线限制加速度和加加速度，使 BLDC 驱动轮启停更柔顺；同时用航点队列管理任务路径，使 AGV 能依次跟踪多个目标点，并在到达、超时、避障或任务变更时自动切换下一航点。

一、主要特点

1. S 曲线规划器抑制启停冲击
   传统梯形速度曲线在加速开始和结束瞬间存在加速度突变，容易引起货物晃动、车轮打滑、机械振动和定位超调。S 型曲线通过限制加加速度 Jerk，使加速度连续变化，速度曲线呈 S 形，从而让 AGV 的启动、减速和停靠更柔顺。
2. 支持多工况自适应
   实际仓库中，航点间距并不固定。长距离可运行完整七段式 S 曲线；中短距离可能没有匀速段，形成五段轨迹；极短距离甚至只有三段柔性过渡。规划器需要根据剩余距离、当前速度、最大速度和最大加速度动态选择轨迹形态。
3. 航点队列提供顺序任务执行能力
   航点队列通常维护一组目标点，例如坐标、姿态、任务类型和到达容差。AGV 每到达一个航点，就自动弹出当前任务并切换到下一个航点。队列还可以支持插入、暂停、取消、重排序和优先级切换。
4. 纵向速度控制与横向路径跟踪分离
   S 曲线规划器主要解决“跑多快”的问题，即纵向速度参考；航点队列和路径跟踪算法解决“往哪走”的问题，例如纯追踪、差速纠偏或简单航向角控制。两者结合后，AGV 既能平滑加减速，又能稳定跟踪路径。
5. 适合 Arduino/ESP32 嵌入式实现
   S 曲线规划可以离散化为每个控制周期更新一次参考速度；航点队列可以用数组或环形缓冲区实现。对于简单差速 AGV，Arduino Uno/Nano 可做原型验证；若同时运行 FOC、编码器测速、IMU 融合和多航点跟踪，建议使用 ESP32、Teensy 或 STM32，并把核心速度环放在固定周期中断中。

二、应用场景

1. 仓储搬运 AGV
   在货架区、拣选区和装卸区之间按固定路线运行。S 曲线可减少托盘货物晃动，航点队列可管理多个装卸点。
2. 产线物料配送
   沿工位依次停靠，完成上料、下料或空箱回收。航点队列可支持任务动态插入，例如紧急补料任务优先执行。
3. 室内外复合导航原型车
   室内可用二维码、UWB、视觉标签或里程计定位，室外可用 GPS/RTK。航点队列适合管理一系列地理坐标或局部坐标点。
4. 教学与科研平台
   适合验证速度规划、路径跟踪、队列调度、到达判定、任务超时处理和异常恢复。
5. 轻载巡检/安防小车
   在固定巡逻路线上依次经过多个检查点。S 曲线可降低低速启停时的抖动，提高摄像头画面稳定性。

三、需要注意的事项

1. S 曲线参数必须与机械和负载匹配
   最大速度、最大加速度和最大加加速度不能只凭经验设置。负载越大、重心越高、轮地摩擦越低，越应降低加速度和 Jerk，否则容易打滑、货物倾倒或定位超调。
2. 必须覆盖短距离工况
   如果只实现完整七段式 S 曲线，遇到相邻航点很近时会出现速度跳变、减速不及时或过冲。工程上应提前判定是否能达到最大加速度和最大速度，必要时切换到五段或三段轨迹。
3. 航点到达判定要有容差和去抖
   仅用“距离等于 0”判断到达并不稳定，传感器噪声会导致航点反复切换。应设置位置容差、姿态容差和稳定时间，例如连续若干周期误差都小于阈值才认为到达。
4. 队列变更必须保证状态连续
   插入新任务、取消当前任务或紧急避障时，不能直接把目标速度置零或突变。应从当前实际速度继续生成新的 S 曲线，避免速度指令跳变造成冲击。
5. 定位误差会直接影响队列切换可靠性
   如果里程计漂移、二维码漏读或 GPS 抖动严重，AGV 可能误判“已到达”或“永远无法到达”。建议融合编码器、IMU 和外部定位源，并对航点切换加入超时保护。
6. BLDC 执行层需要闭环和保护
   S 曲线输出的是参考速度或参考位置，底层仍需速度环/位置环跟踪。应限制电机电流、监测堵转、处理编码器异常，并在通信丢失或传感器失效时进入安全停机。
7. Arduino 算力要合理分配
   S 曲线更新、航点判定、路径跟踪、FOC 和传感器读取同时运行时，主循环延迟会显著影响控制质量。建议把速度规划放在固定周期任务中，减少串口打印和动态内存分配；复杂调度可放到上位机或更高性能 MCU。
   整体链路可以概括为：航点队列给出目标点 → 路径跟踪计算期望航向和速度上限 → S 曲线规划器生成平滑速度参考 → BLDC 速度/位置闭环执行 → 到达判定触发下一航点。它的工程价值在于：S 曲线解决“动得稳”，航点队列解决“走得顺”，两者结合后更适合仓库 AGV 这种频繁启停、多目标点、带载运行的场景。

1、基础航点队列 + S 曲线速度规划
此案例建立仓库 AGV 最核心的框架：从队列中取出目标航点，用 S 曲线规划器生成平滑的速度指令，驱动 BLDC 电机执行。

#include <SimpleFOC.h>

// ==================== BLDC 电机与驱动 ====================
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 4);

// ==================== 航点队列结构 ====================
struct Waypoint {
    long encoderTarget;   // 该航点对应的编码器目标位置
    uint8_t flags;        // 预留标志位
};

// 环形队列实现航点管理
#define QUEUE_SIZE 16
Waypoint waypointQueue[QUEUE_SIZE];
uint8_t queueHead = 0;    // 出队索引
uint8_t queueTail = 0;    // 入队索引
uint8_t queueCount = 0;   // 当前队列长度

bool enqueueWaypoint(long target) {
    if (queueCount >= QUEUE_SIZE) return false;
    waypointQueue[queueTail].encoderTarget = target;
    queueTail = (queueTail + 1) % QUEUE_SIZE;
    queueCount++;
    return true;
}

bool dequeueWaypoint(Waypoint &wp) {
    if (queueCount == 0) return false;
    wp = waypointQueue[queueHead];
    queueHead = (queueHead + 1) % QUEUE_SIZE;
    queueCount--;
    return true;
}

// ==================== S 曲线规划器（七段式） ====================
struct SCurveProfile {
    float pos;           // 当前位置
    float vel;           // 当前速度
    float acc;           // 当前加速度
    
    float maxVel;        // 最大速度限制
    float maxAcc;        // 最大加速度限制
    float maxJerk;       // 最大加加速度限制
    
    float targetPos;     // 目标位置
    bool  active;        // 规划器激活标志
};

SCurveProfile planner;

void initSCurve(SCurveProfile &p, float maxV, float maxA, float maxJ) {
    p.pos = 0;
    p.vel = 0;
    p.acc = 0;
    p.maxVel = maxV;
    p.maxAcc = maxA;
    p.maxJerk = maxJ;
    p.targetPos = 0;
    p.active = false;
}

// S 曲线核心更新函数（简化七段式，基于 jerk 限制）
void updateSCurve(SCurveProfile &p, float dt) {
    if (!p.active) return;
    
    float distToGoal = p.targetPos - p.pos;
    float distAbs = fabs(distToGoal);
    float velSign = (distToGoal > 0) ? 1.0f : -1.0f;
    
    // 计算当前速度下所需的制动距离
    float stopDist = (p.vel * p.vel) / (2.0f * p.maxAcc);
    
    // 根据距离选择 jerk 符号
    if (distAbs <= stopDist && fabs(p.vel) > 0.01f) {
        // 进入减速区：施加负 jerk 或负加速度
        p.acc -= p.maxJerk * dt * velSign;
    } else if (fabs(p.vel) < p.maxVel) {
        // 加速区：施加正 jerk
        p.acc += p.maxJerk * dt * velSign;
    } else {
        // 匀速区：加速度趋向 0
        p.acc *= 0.95f;
    }
    
    // 约束加速度和速度
    p.acc = constrain(p.acc, -p.maxAcc, p.maxAcc);
    p.vel += p.acc * dt;
    p.vel = constrain(p.vel, -p.maxVel, p.maxVel);
    p.pos += p.vel * dt;
    
    // 到达目标附近，停止规划
    if (distAbs < 1.0f && fabs(p.vel) < 0.1f) {
        p.vel = 0;
        p.acc = 0;
        p.active = false;
        Serial.println("Waypoint reached");
    }
}

// ==================== 主控制循环 ====================
unsigned long lastTime = 0;
Waypoint currentWaypoint;

void setup() {
    Serial.begin(115200);
    
    // 初始化电机
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init();
    motorR.init();
    motorL.initFOC();
    motorR.initFOC();
    
    // 初始化 S 曲线规划器
    initSCurve(planner, 200.0f, 300.0f, 500.0f);
    
    // 加载示例航点队列（工位1→2→3→4）
    enqueueWaypoint(1000);
    enqueueWaypoint(1800);
    enqueueWaypoint(2600);
    enqueueWaypoint(3400);
    
    Serial.println("AGV ready, waypoints loaded");
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    unsigned long now = micros();
    float dt = (now - lastTime) / 1e6f;
    if (dt < 0.001f) return;  // 最小 1ms 周期
    lastTime = now;
    
    // 1. 如果规划器空闲，从队列取下一个航点
    if (!planner.active) {
        if (dequeueWaypoint(currentWaypoint)) {
            planner.targetPos = currentWaypoint.encoderTarget;
            planner.active = true;
            Serial.print("Moving to: ");
            Serial.println(planner.targetPos);
        } else {
            // 队列为空，AGV 停止
            motorL.move(0);
            motorR.move(0);
            return;
        }
    }
    
    // 2. 更新 S 曲线规划器
    updateSCurve(planner, dt);
    
    // 3. 将规划器速度指令发给 BLDC
    float cmdVel = planner.vel * 0.1f;  // 缩放为合适的速度指令
    motorL.move(cmdVel);
    motorR.move(cmdVel);
    
    delay(5);
}

要点：队列的环形结构保证了航点管理的确定性和内存效率。S 曲线规划器在制动距离判断处决定是否开始减速，这比简单的“距离小于阈值就减速”更精确，能避免过冲。

2、多工位自适应 S 曲线（载重/工位类型驱动）
此案例引入工位类型感知，不同工位（宽通道/窄通道/重载区）使用不同的 S 曲线参数，实现自适应加减速。

#include <SimpleFOC.h>

// ==================== BLDC 与编码器 ====================
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 4);

// ==================== 工位参数表 ====================
enum StationType {
    STATION_WIDE,      // 宽通道：高速通过
    STATION_NARROW,    // 窄通道：低速精定位
    STATION_HEAVY,     // 重载区：低速大减速度
    STATION_CHARGER    // 充电站：极低速对接
};

struct StationProfile {
    StationType type;
    float maxVel;      // 该工位区间的最大速度
    float maxAcc;      // 该工位区间的最大加速度
    float maxJerk;     // 该工位区间的最大加加速度
};

// 航点结构扩展：绑定工位类型
struct Waypoint {
    long encoderTarget;
    StationType stationType;
};

StationProfile stationProfiles[] = {
    {STATION_WIDE,    300.0f, 400.0f, 600.0f},
    {STATION_NARROW,  120.0f, 200.0f, 350.0f},
    {STATION_HEAVY,   100.0f, 150.0f, 250.0f},
    {STATION_CHARGER,  50.0f,  80.0f, 120.0f}
};

// ==================== 队列 ====================
#define QUEUE_SIZE 16
Waypoint waypointQueue[QUEUE_SIZE];
uint8_t queueHead = 0, queueTail = 0, queueCount = 0;

bool enqueueWaypoint(long target, StationType type) {
    if (queueCount >= QUEUE_SIZE) return false;
    waypointQueue[queueTail] = {target, type};
    queueTail = (queueTail + 1) % QUEUE_SIZE;
    queueCount++;
    return true;
}

bool dequeueWaypoint(Waypoint &wp) {
    if (queueCount == 0) return false;
    wp = waypointQueue[queueHead];
    queueHead = (queueHead + 1) % QUEUE_SIZE;
    queueCount--;
    return true;
}

// ==================== 自适应 S 曲线规划器 ====================
struct SCurvePlanner {
    float pos, vel, acc;
    float maxVel, maxAcc, maxJerk;
    float targetPos;
    bool active;
};

SCurvePlanner planner;

void applyProfile(SCurvePlanner &p, StationType type) {
    for (auto &prof : stationProfiles) {
        if (prof.type == type) {
            p.maxVel = prof.maxVel;
            p.maxAcc = prof.maxAcc;
            p.maxJerk = prof.maxJerk;
            Serial.print("Profile applied - Vel:");
            Serial.print(p.maxVel);
            Serial.print(" Acc:");
            Serial.println(p.maxAcc);
            return;
        }
    }
}

void updateSCurve(SCurvePlanner &p, float dt) {
    if (!p.active) return;
    
    float distToGoal = p.targetPos - p.pos;
    float distAbs = fabs(distToGoal);
    float velSign = (distToGoal > 0) ? 1.0f : -1.0f;
    
    // 精确制动距离判断
    float stopDist = (p.vel * p.vel) / (2.0f * p.maxAcc);
    
    if (distAbs <= stopDist && fabs(p.vel) > 0.01f) {
        p.acc -= p.maxJerk * dt * velSign;
    } else if (fabs(p.vel) < p.maxVel) {
        p.acc += p.maxJerk * dt * velSign;
    } else {
        p.acc *= 0.95f;
    }
    
    p.acc = constrain(p.acc, -p.maxAcc, p.maxAcc);
    p.vel += p.acc * dt;
    p.vel = constrain(p.vel, -p.maxVel, p.maxVel);
    p.pos += p.vel * dt;
    
    if (distAbs < 1.0f && fabs(p.vel) < 0.1f) {
        p.vel = 0; p.acc = 0; p.active = false;
    }
}

// ==================== 主循环 ====================
unsigned long lastTime = 0;
Waypoint currentWp;

void setup() {
    Serial.begin(115200);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    planner = {0, 0, 0, 200.0f, 300.0f, 500.0f, 0, false};
    
    // 加载混合类型航点队列
    enqueueWaypoint(1000, STATION_WIDE);
    enqueueWaypoint(1800, STATION_NARROW);
    enqueueWaypoint(2600, STATION_HEAVY);
    enqueueWaypoint(3400, STATION_CHARGER);
    
    Serial.println("Multi-profile AGV ready");
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    unsigned long now = micros();
    float dt = (now - lastTime) / 1e6f;
    if (dt < 0.001f) return;
    lastTime = now;
    
    if (!planner.active) {
        if (dequeueWaypoint(currentWp)) {
            // 根据航点类型应用对应的 S 曲线参数
            applyProfile(planner, currentWp.stationType);
            planner.targetPos = currentWp.encoderTarget;
            planner.active = true;
        } else {
            motorL.move(0); motorR.move(0);
            return;
        }
    }
    
    updateSCurve(planner, dt);
    
    float cmdVel = planner.vel * 0.1f;
    motorL.move(cmdVel);
    motorR.move(cmdVel);
    
    delay(5);
}

要点：不同工位使用不同的 maxVel/maxAcc/maxJerk 组合，使得 AGV 在宽通道可以高速通过，进入窄通道或重载区时自动降速。这比全局统一的 S 曲线参数更符合实际仓储场景。

3、带优先级抢占的航点队列 + 动态重规划
此案例处理仓库中的紧急任务插入：高优先级任务可以抢占当前航点队列，AGV 在安全停车后重新规划。

#include <SimpleFOC.h>

// ==================== BLDC ====================
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 4);

// ==================== 优先级航点 ====================
struct PriorityWaypoint {
    long encoderTarget;
    uint8_t priority;     // 0=普通, 1=加急, 2=紧急
    uint32_t timestamp;   // 入队时间，用于同优先级排序
};

#define QUEUE_SIZE 20
PriorityWaypoint queue[QUEUE_SIZE];
uint8_t head = 0, tail = 0, count = 0;

// 按优先级插入（非尾部追加）
bool enqueuePriority(long target, uint8_t prio) {
    if (count >= QUEUE_SIZE) return false;
    
    // 简单实现：找到第一个优先级更低的位置插入
    // 实际可用二叉堆或有序链表优化
    uint8_t pos = tail;
    for (uint8_t i = 0; i < count; i++) {
        uint8_t idx = (head + i) % QUEUE_SIZE;
        if (queue[idx].priority < prio) {
            pos = idx;
            break;
        }
    }
    
    if (pos == tail) {
        // 追加到尾部
        queue[tail] = {target, prio, millis()};
        tail = (tail + 1) % QUEUE_SIZE;
        count++;
        return true;
    }
    
    // 插入到 pos 位置：需要后移元素
    // 简化处理：仅示例逻辑，实际需环形数组的插入操作
    queue[tail] = {target, prio, millis()};
    tail = (tail + 1) % QUEUE_SIZE;
    count++;
    return true;
}

// ==================== S 曲线（支持紧急制动） ====================
struct SCurvePlanner {
    float pos, vel, acc;
    float maxVel, maxAcc, maxJerk;
    float targetPos;
    bool active;
    bool emergencyStop;
};

SCurvePlanner planner;

void updateSCurve(SCurvePlanner &p, float dt) {
    if (!p.active) return;
    
    float distToGoal = p.targetPos - p.pos;
    float distAbs = fabs(distToGoal);
    float velSign = (distToGoal > 0) ? 1.0f : -1.0f;
    
    // 紧急停止：强制减速度
    if (p.emergencyStop) {
        p.acc = -velSign * p.maxAcc;
        p.emergencyStop = false;  // 仅执行一次
    } else {
        float stopDist = (p.vel * p.vel) / (2.0f * p.maxAcc);
        if (distAbs <= stopDist && fabs(p.vel) > 0.01f) {
            p.acc -= p.maxJerk * dt * velSign;
        } else if (fabs(p.vel) < p.maxVel) {
            p.acc += p.maxJerk * dt * velSign;
        } else {
            p.acc *= 0.95f;
        }
    }
    
    p.acc = constrain(p.acc, -p.maxAcc, p.maxAcc);
    p.vel += p.acc * dt;
    p.vel = constrain(p.vel, -p.maxVel, p.maxVel);
    p.pos += p.vel * dt;
    
    if (distAbs < 1.0f && fabs(p.vel) < 0.1f) {
        p.vel = 0; p.acc = 0; p.active = false;
    }
}

// ==================== 抢占逻辑 ====================
bool preemptTask(long newTarget, uint8_t newPriority) {
    // 检查当前正在执行的任务优先级
    if (planner.active && newPriority >= 2) {
        // 紧急任务：触发紧急停车
        planner.emergencyStop = true;
        Serial.println("EMERGENCY PREEMPT - stopping");
        
        // 等待停车完成
        while (planner.active) {
            motorL.loopFOC();
            motorR.loopFOC();
            updateSCurve(planner, 0.001f);
            motorL.move(planner.vel * 0.1f);
            motorR.move(planner.vel * 0.1f);
            delay(1);
        }
        
        // 清空队列，优先执行紧急任务
        head = 0; tail = 0; count = 0;
        enqueuePriority(newTarget, newPriority);
        return true;
    }
    // 加急任务：插入队列头部
    else if (newPriority >= 1) {
        enqueuePriority(newTarget, newPriority);
        return true;
    }
    return false;
}

// ==================== 主循环 ====================
unsigned long lastTime = 0;

void setup() {
    Serial.begin(115200);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    planner = {0, 0, 0, 200.0f, 300.0f, 500.0f, 0, false, false};
    
    // 普通任务队列
    enqueuePriority(1000, 0);
    enqueuePriority(1800, 0);
    enqueuePriority(2600, 0);
    
    Serial.println("Priority queue AGV ready");
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    unsigned long now = micros();
    float dt = (now - lastTime) / 1e6f;
    if (dt < 0.001f) return;
    lastTime = now;
    
    // 模拟串口接收紧急任务
    if (Serial.available()) {
        char cmd = Serial.read();
        if (cmd == 'E') {
            preemptTask(5000, 2);  // 紧急任务到 5000
        } else if (cmd == 'U') {
            enqueuePriority(4200, 1);  // 加急任务
        }
    }
    
    if (!planner.active) {
        if (count > 0) {
            planner.targetPos = queue[head].encoderTarget;
            head = (head + 1) % QUEUE_SIZE;
            count--;
            planner.active = true;
            Serial.print("Executing target: ");
            Serial.println(planner.targetPos);
        } else {
            motorL.move(0); motorR.move(0);
            return;
        }
    }
    
    updateSCurve(planner, dt);
    
    motorL.move(planner.vel * 0.1f);
    motorR.move(planner.vel * 0.1f);
    
    delay(5);
}

要点：紧急任务抢占时，系统先触发紧急停车（强制最大减速度），等待完全停止后再清空队列执行紧急任务。这保证了抢占过程的安全性和确定性。

要点解读

1. S 曲线的核心价值是“消除加加速度突变”
   梯形速度曲线的加速度是阶跃变化的，这意味着加加速度（jerk）在切换点无穷大，会导致机械冲击和货物晃动。S 曲线通过限制 jerk 的幅值，使加速度连续变化，速度曲线呈 S 形。对于满载货物的 AGV，这意味着货物不会因急加速而倾倒，也不会因急减速而前冲。

2. 制动距离判断是 S 曲线规划器的“刹车时机”
   朴素实现用“剩余距离 < 阈值”触发减速，但正确的做法是用 v²/(2a) 计算当前速度下需要的制动距离。只有当剩余距离小于制动距离时才开始减速。这避免了高速时减速太晚导致过冲，也避免了低速时过早减速影响效率。

3. 航点队列的环形结构是嵌入式的最优选择
   用 head/tail/count 三个变量管理的环形队列，入队和出队都是 O(1) 操作，无需动态内存分配。对于 Arduino 的有限 RAM 环境，这比 std::queue 或链表更可靠。队列满时的行为需要明确定义：拒绝入队还是覆盖最旧的元素。

4. 工位类型绑定 S 曲线参数是“自适应”的落地方式
   全局统一的 S 曲线参数无法兼顾宽通道的高速需求和充电站的精定位需求。将 maxVel/maxAcc/maxJerk 与工位类型绑定，AGV 在进入不同区域时自动切换参数。这比运行时动态调整参数更简单、更可预测。

5. 优先级抢占需要“安全停车”作为前置条件
   紧急任务不能直接覆盖当前规划器的目标位置，这会导致 S 曲线规划器在高速时突然反向，产生巨大冲击。正确流程是：触发紧急停车 → 等待速度归零 → 清空/重排队列 → 启动新任务。

4、基于时间积分的 S 曲线规划器（jerk 限幅）
不用预先展开七段解析式，而是用"制动距离反算目标速度 + 加速度斜率限幅"的实时积分法。它对任意距离都成立，也能在中途改变目标，最适合嵌入式。

#include <SimpleFOC.h>

/* ================= 单轴 BLDC 速度环 ================= */
BLDCMotor      motor(7);
BLDCDriver3PWM drv(9, 10, 11, 8);
Encoder        enc(2, 3, 2048);

const float WHEEL_D = 150.0f;                     // mm
const float TICK_MM = 3.14159265f * WHEEL_D / 2048.0f;
const float REDUCTION = 20.0f;                    // 减速比

/* ================= S 曲线参数 ================= */
struct SCurve {
  float vMax;      // 最大速度 mm/s
  float aMax;      // 最大加速度 mm/s²
  float jMax;      // 最大加加速度 mm/s³
  float d;         // 本段总距离 mm
  float s;         // 已行进距离 mm
  float v;         // 当前速度 mm/s
  float a;         // 当前加速度 mm/s²
  float dir;       // +1 / -1
  bool  active;
};

SCurve g;

/* 载重后须下调的限值——这是 AGV 与普通小车的关键差异 */
const float V_MAX_LOADED = 600.0f;    // 满载 0.6 m/s
const float A_MAX_LOADED = 350.0f;    // 约 0.036 g，防货物滑移
const float J_MAX_LOADED = 1800.0f;   // 冲击限制

void scStart(SCurve &c, float dist, float vM, float aM, float jM) {
  c.vMax = vM; c.aMax = aM; c.jMax = jM;
  c.d = fabsf(dist); c.dir = (dist >= 0) ? 1.0f : -1.0f;
  c.s = 0; c.v = 0; c.a = 0;
  c.active = true;
}

/* 单步推进：必须由固定周期调用，dt 用实测值 */
void scUpdate(SCurve &c, float dt) {
  if (!c.active) return;

  float rem = c.d - c.s;
  if (rem <= 0.0f) { c.v = 0; c.a = 0; c.active = false; return; }

  /* 1) 由剩余距离反算"制动允许的最高速度" */
  float vBrake = sqrtf(2.0f * c.aMax * rem);
  float vTgt   = fminf(c.vMax, vBrake);

  /* 2) 计算达成该速度所需的加速度，并做加速度限幅 */
  float aTgt = (vTgt - c.v) / dt;
  aTgt = constrain(aTgt, -c.aMax, c.aMax);
  if (vTgt <= 0.0f && c.v < 1.0f) aTgt = -c.v / dt;   // 收尾段平稳归零

  /* 3) jerk 限幅——S 曲线的核心，加速度变化率受限 */
  float aStep = c.jMax * dt;
  c.a += constrain(aTgt - c.a, -aStep, aStep);

  /* 4) 积分得到速度与位移 */
  c.v += c.a * dt;
  if (c.v < 0.0f) { c.v = 0.0f; c.a = 0.0f; }
  c.s += c.v * dt;

  /* 5) 防止过冲 */
  if (c.s > c.d) { c.s = c.d; c.v = 0; c.a = 0; }
}

/* ================= 指令下发 ================= */
float cmdVel = 0.0f;                  // 本轴当前目标速度 mm/s

void applyCommand(SCurve &c) {
  float v = c.dir * c.v;
  // mm/s -> 电机轴 rad/s
  float motorVel = v / TICK_MM / REDUCTION;
  motor.move(motorVel);
}

/* ================= 里程反馈闭环 ================= */
float shaftPos_mm = 0.0f;

void updateFeedback() {
  // 用编码器角度反推实际位移，修正积分漂移
  static float lastAng = 0.0f;
  float ang = motor.shaft_angle;
  float dAng = ang - lastAng;
  lastAng = ang;
  shaftPos_mm += dAng * TICK_MM * REDUCTION;
}

void setup() {
  Serial.begin(115200);
  enc.init();
  drv.voltage_power_supply = 48;
  drv.init();
  motor.linkDriver(&drv);
  motor.linkSensor(&enc);
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller        = MotionControlType::velocity;
  motor.voltage_limit     = 24.0f;
  motor.velocity_limit    = 60.0f;
  motor.PID_velocity.P    = 0.4f;
  motor.PID_velocity.I    = 2.0f;
  motor.PID_velocity.D    = 0.0f;
  motor.LPF_velocity.Tf   = 0.02f;
  motor.init();
  motor.initFOC();

  scStart(g, 2000.0f, V_MAX_LOADED, A_MAX_LOADED, J_MAX_LOADED);
  Serial.println(F("S-curve ready"));
}

void loop() {
  motor.loopFOC();
  updateFeedback();

  static uint32_t tPrev = 0;
  uint32_t now = micros();
  if (now - tPrev >= 5000) {                    // 200 Hz 规划环
    float dt = (now - tPrev) / 1e6f;
    tPrev = now;
    if (dt > 0.05f) dt = 0.05f;                 // 防止长时间抖动导致跳变

    scUpdate(g, dt);
    applyCommand(g);

    if (!g.active) {
      static bool once = false;
      if (!once) { once = true; Serial.println(F("segment done")); }
    }
  }
  delayMicroseconds(200);
}

第 1 步的 vBrake = √(2·aMax·rem) 是整个规划器的灵魂：它让"该减速了"这个判断从时间问题变成距离问题，因此对任意长的段、任意中途变更的目标都天然成立。第 3 步的 jerk 限幅才是 S 曲线区别于梯形曲线的真正标志——aTgt 不是直接赋值给加速度，而是以每周期 jMax·dt 的斜率逼近。

5、航点队列管理（环形缓冲 + 非阻塞导航状态机）
队列要解决的是三件事：存、取、以及"什么时候算到了"。环形缓冲保证 O(1) 入队出队、无动态分配；状态机保证导航过程永不阻塞主循环。

/* ================= 航点定义 ================= */
struct Waypoint {
  float x, y;            // 世界坐标 mm
  float theta;           // 目标朝向 rad（NAN = 不关心朝向）
  float vMax;            // 本段限速 mm/s（0 = 用默认值）
  uint8_t action;        // 0=通过 1=停稳 2=举升 3=等待
  uint16_t dwellMs;      // action=1/3 时的停留时间
};

#define Q_SIZE 32                      // 必须是 2 的幂，便于位掩码取模
const uint8_t Q_MASK = Q_SIZE - 1;

Waypoint qbuf[Q_SIZE];
volatile uint8_t qHead = 0;            // 出队位置
volatile uint8_t qTail = 0;            // 入队位置
volatile uint8_t qCount = 0;

bool qPush(const Waypoint &wp) {
  if (qCount >= Q_SIZE) return false;  // 队满，调用方须处理
  qbuf[qTail] = wp;
  qTail = (qTail + 1) & Q_MASK;
  qCount++;
  return true;
}

bool qPop(Waypoint &wp) {
  if (qCount == 0) return false;
  wp = qbuf[qHead];
  qHead = (qHead + 1) & Q_MASK;
  qCount--;
  return true;
}

bool qPeek(Waypoint &wp) {
  if (qCount == 0) return false;
  wp = qbuf[qHead];
  return true;
}

void qClear() { qHead = qTail = qCount = 0; }

/* 插队：把紧急任务放到队首（叉车让行、急停返航等） */
bool qPushFront(const Waypoint &wp) {
  if (qCount >= Q_SIZE) return false;
  qHead = (qHead - 1) & Q_MASK;
  qbuf[qHead] = wp;
  qCount++;
  return true;
}

/* ================= 定位与朝向 ================= */
float poseX = 0.0f, poseY = 0.0f, poseTh = 0.0f;   // 由里程计/磁条/QR 校正提供

/* ================= 导航状态机 ================= */
enum NavState { NAV_IDLE, NAV_ALIGN, NAV_CRUISE, NAV_DWELL, NAV_FAULT };
NavState nav = NAV_IDLE;

Waypoint curWp;
float  segDist   = 0.0f;     // 本段直线距离
float  segHeading= 0.0f;     // 本段航向
uint32_t tDwell  = 0;

const float ARRIVE_TOL_MM = 15.0f;     // 到达位置容差
const float ARRIVE_TOL_RAD = 0.035f;   // 约 2°

/* 供案例一使用的规划器实例 */
extern SCurve g;
extern void scStart(SCurve&, float, float, float, float);
extern void scUpdate(SCurve&, float);
extern bool scDone();
extern float scVel();

bool scDone() { return !g.active; }
float scVel() { return g.v; }

/* 转向控制：用差速原地旋转（示意，实际接差速层） */
float alignErr = 0.0f;
bool  alignDone = false;
void alignTo(float theta, float dt);     // 由案例三实现

/* ================= 状态机主循环 ================= */
void navUpdate(float dt) {
  switch (nav) {

    case NAV_IDLE:
      if (qPeek(curWp)) {
        float dx = curWp.x - poseX;
        float dy = curWp.y - poseY;
        segDist    = sqrtf(dx * dx + dy * dy);
        segHeading = atan2f(dy, dx);
        if (segDist < ARRIVE_TOL_MM) {   // 已在点上，直接处理动作
          nav = NAV_DWELL; tDwell = millis();
        } else {
          nav = NAV_ALIGN;
        }
      }
      break;

    case NAV_ALIGN:
      /* 先转向、后直线：差速车最稳的做法 */
      alignTo(segHeading, dt);
      if (alignDone) {
        float vM = (curWp.vMax > 0) ? curWp.vMax : V_MAX_LOADED;
        scStart(g, segDist, vM, A_MAX_LOADED, J_MAX_LOADED);
        nav = NAV_CRUISE;
      }
      break;

    case NAV_CRUISE:
      scUpdate(g, dt);
      if (scDone()) {
        // 到达后按需原地转向到目标朝向
        if (!isnan(curWp.theta)) {
          alignTo(curWp.theta, dt);
          if (!alignDone) break;
        }
        nav = NAV_DWELL;
        tDwell = millis();
      }
      break;

    case NAV_DWELL:
      /* 执行动作，期间保持停稳 */
      if (curWp.action == 2) doLift();          // 举升等业务动作
      if (millis() - tDwell >= curWp.dwellMs) {
        qPop(curWp);                            // 出队，进入下一航点
        nav = (qCount > 0) ? NAV_IDLE : NAV_IDLE;
        Serial.print(F("waypoint done, remain=")); Serial.println(qCount);
      }
      break;

    case NAV_FAULT:
      /* 卡死/定位丢失：停车并上报 */
      stopAll();
      break;
  }
}

NAV_ALIGN 与 NAV_CRUISE 分离是差速 AGV 的关键设计。如果边走边转，位置精度会被转向误差污染，尤其在窄巷道里；先转到位再走直线，能最大限度利用里程计的直线精度。

6、双轮 BLDC 集成 —— S 曲线 + 队列 + 转角优化与动态插队
把前两案合起来，并处理真正的多段问题：段间衔接不能每段都停稳（否则效率极低），连续直角弯要用"减速通过"而非"完全停止"。

#include <SimpleFOC.h>

/* ================= 双轮 BLDC ================= */
BLDCMotor mL(7),  mR(7);
BLDCDriver3PWM dL(9,10,11,8), dR(5,6,3,4);
Encoder eL(2,18,2048), eR(19,20,2048);

const float WHEEL_D = 150.0f;
const float TRACK   = 400.0f;                     // 轮距 mm
const float TICK_MM = 3.14159265f * WHEEL_D / 2048.0f;
const float REDUCT  = 20.0f;

/* ================= 里程计 ================= */
float poseX = 0, poseY = 0, poseTh = 0;
uint32_t tOdom = 0;

void updateOdom() {
  uint32_t now = micros();
  float dt = (now - tOdom) / 1e6f;
  if (dt <= 0 || dt > 0.1f) { tOdom = now; return; }
  tOdom = now;

  float vl = mL.shaft_velocity * TICK_MM * REDUCT;   // mm/s
  float vr = mR.shaft_velocity * TICK_MM * REDUCT;
  float v = (vl + vr) * 0.5f;
  float w = (vr - vl) / TRACK;                       // rad/s

  poseTh += w * dt;
  poseX  += v * cosf(poseTh) * dt;
  poseY  += v * sinf(poseTh) * dt;
}

/* ================= 差速速度映射 ================= */
float vCmd = 0.0f, wCmd = 0.0f;

void driveDiff(float v, float w) {
  // 限幅：先限角速度保证几何，再等比缩线速度避免超限
  float vl = v - w * TRACK * 0.5f;
  float vr = v + w * TRACK * 0.5f;
  float m = fmaxf(fabsf(vl), fabsf(vr));
  const float V_LIMIT = V_MAX_LOADED * 1.2f;
  if (m > V_LIMIT) { vl *= V_LIMIT / m; vr *= V_LIMIT / m; }

  mL.move(vl / (TICK_MM * REDUCT));
  mR.move(vr / (TICK_MM * REDUCT));
}

/* ================= 转向对齐（P 控制 + 最小速度死区） ================= */
float alignErr = 0.0f;
bool  alignDone = false;

void alignTo(float theta, float dt) {
  float err = theta - poseTh;
  while (err >  PI) err -= 2*PI;
  while (err < -PI) err += 2*PI;
  alignErr = err;

  const float KP_W = 3.0f;
  const float W_MIN = 0.25f;                  // 死区，防低速卡住
  const float W_MAX = 1.2f;

  if (fabsf(err) < ARRIVE_TOL_RAD) {
    alignDone = true;
    driveDiff(0, 0);
    return;
  }
  alignDone = false;
  float w = constrain(KP_W * err, -W_MAX, W_MAX);
  if (fabsf(w) < W_MIN) w = (w > 0) ? W_MIN : -W_MIN;
  driveDiff(0, w);                            // 原地旋转
}

/* ================= 连续弯道：前瞻判断能否不停车通过 ================= */
extern bool qPeekNext(Waypoint &wp);          // 查看队列里下一个点

const float CORNER_ANGLE_TH = 0.5f;           // 约 28°，超过则需减速

/* 若当前段尽头有急转，提前把本段限速压低 */
float effectiveVMax() {
  Waypoint next;
  if (qCount >= 2 && qPeekNext(next)) {
    float dx = next.x - curWp.x, dy = next.y - curWp.y;
    float h2 = atan2f(dy, dx);
    float dTh = fabsf(h2 - segHeading);
    while (dTh > PI) dTh = fabsf(dTh - 2*PI);
    if (dTh > CORNER_ANGLE_TH) {
      // 转角越大，限速越低：简单线性映射（工程上可查表）
      float scale = 1.0f - 0.6f * (dTh / PI);
      return V_MAX_LOADED * fmaxf(scale, 0.35f);
    }
  }
  return V_MAX_LOADED;
}

/* ================= 动态插队与重规划 ================= */
void onObstacleDetected(int stopIndex) {
  // 在受影响航点之后插入绕行点，不打断已执行部分
  Waypoint detour = {poseX + 300, poseY - 300, NAN, 400, 0, 0};
  if (!qPush(detour)) { nav = NAV_FAULT; reportFault(FAULT_Q_FULL); }
}

void onEmergencyReturn() {
  Waypoint home = {0, 0, 0, 500, 1, 500};     // 返航并停稳
  qClear();                                    // 清空当前任务
  qPushFront(home);
  nav = NAV_IDLE;
}

/* ================= 主循环 ================= */
void setup() {
  Serial.begin(115200);
  eL.init(); eR.init();
  dL.voltage_power_supply = 48; dR.voltage_power_supply = 48;
  dL.init(); dR.init();
  mL.linkDriver(&dL); mR.linkDriver(&dR);
  mL.linkSensor(&eL); mR.linkSensor(&eR);
  mL.controller = mR.controller = MotionControlType::velocity;
  mL.voltage_limit = mR.voltage_limit = 24.0f;
  mL.PID_velocity.P = mR.PID_velocity.P = 0.4f;
  mL.PID_velocity.I = mR.PID_velocity.I = 2.0f;
  mL.init(); mR.init(); mL.initFOC(); mR.initFOC();
  tOdom = micros();
  Serial.println(F("AGV ready"));
}

void loop() {
  mL.loopFOC(); mR.loopFOC();
  updateOdom();

  static uint32_t tNav = 0;
  uint32_t now = micros();
  if (now - tNav >= 5000) {
    float dt = (now - tNav) / 1e6f;
    tNav = now;
    if (dt > 0.05f) dt = 0.05f;

    navUpdate(dt);                    // 案例二的状态机

    // 巡航阶段由 S 曲线驱动，并把前瞻限速注入
    if (nav == NAV_CRUISE) {
      float v = scVel();
      // 前馈：本段距离不足时，S 曲线已自动减速
      driveDiff(v, 0.0f);
    }
  }
  delayMicroseconds(200);
}

effectiveVMax() 解决的是仓库场景里最现实的问题：90° 直角弯如果每段都停稳再起步，一趟任务会多花几倍时间；但完全不减速直接过弯，又会因向心加速度使货物侧滑。折中做法是在段末前瞻下一段转角，把本段限速预先压低——让 S 曲线在"到达即转向"时刚好降到可接受速度。

要点解读
第一，S 曲线的本质是 jerk 限幅，而不是"梯形曲线加个圆角"。 加速度的跳变会以冲击载荷的形式传到货物、减速箱和轮系上。仓库货物多为托盘堆垛，横向冲击一旦超过摩擦极限就会侧滑；减速比大、齿隙存在的摆线针轮或行星减速箱也忌讳反复冲击。所以 jMax 不是可选的调优参数，而是由"货物防滑"和"机械寿命"倒推出来的硬约束。它的量纲是 mm/s³，物理含义是"加速度每秒能变多少"——jMax 越小，启停越柔和，但段耗时越长。这三者（vMax、aMax、jMax）必须一起标定，且载重变化时都要下调。

第二，规划器必须是固定周期调用的非阻塞积分器，dt 只能来自实测时基。 常见的错误写法是在 S 曲线里塞 delay() 或按"假设循环刚好 5ms"来积分，结果一旦有其他任务抢占（比如串口输出、Modbus 轮询），速度剖面立刻失真。正确做法是记录 micros() 差分、对 dt 做合理上限保护（案例一的 0.05s 截断就是防止长时间卡顿引起位置与速度的数值跳变），并把规划环与 FOC 环分层——FOC 跑几千赫兹，规划环 100–200 Hz 足够。还要注意浮点累积误差：规划器输出的积分位移会缓慢偏离实际，需要用编码器反推的位移做周期性校正，而不是让规划器自说自话。

第三，航点队列是"存"、状态机是"取"，两者都不能有阻塞操作。 环形缓冲是嵌入式队列的标准选择：定长、无动态分配、不会内存碎片、进出都是 O(1)。取模用位掩码而不是 %，在 AVR 上能省下可观的周期。设计上有两个必须想清楚的点：一是队满策略（丢弃最旧、拒绝新点、还是覆盖），AGV 场景通常该拒绝并上报；二是插队能力，应急返航、临时避障都需要"把点放到队首"，所以 qPushFront 得单独实现并在头指针减一时正确处理环绕。状态机的每个状态都必须能在一次调用内返回，绝不允许在 NAV_CRUISE 里 while 等待到点。

第四，段间衔接与到点判定是精度与效率的平衡点，两者都靠容差参数说话。 到达判定给多大的容差，直接决定 AGV 是"永远在微调"还是"错过点位"。位置容差 15mm、角度容差 2° 是常见起步值，但必须与定位精度匹配——如果磁条定位本身有 20mm 误差，设 15mm 容差就会永远判不到点。另一个关键选择是"每段停稳"还是"连续通过"：取放货、窄巷道、指定朝向的点必须停稳（action=1），而长走廊中间的过渡点可以减速通过。案例三的做法是让队列里每个航点自己声明这个属性，而不是全局一刀切。

第五，动态环境下的队列必须可编辑，且安全策略要独立于规划逻辑。 仓库里最真实的情况是"计划赶不上变化"：临时出现的叉车、被占用的巷道、突然下发的急单。系统必须具备在运行中插入绕行点、清空队列返航、调整后续航点顺序的能力，而这些操作不能打断正在执行的段——只改队列尾部，不动已经出发的那一段。与此同时，安全兜底必须独立：急停按钮走硬线直接切断驱动器使能，不经过 MCU；通信超时要有独立的看门狗让 AGV 自动停车；定位长时间无校正信号（磁条丢线、QR 未识别）要降速并告警而不是继续盲走。归根到底，这套系统的可靠性来自"S 曲线保证运动平滑、队列保证任务有序、状态机保证永不死等、独立安全链保证出错能停"这四层各司其职。

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