从嵌入式与多智能体调度角度看，“Arduino BLDC 之送餐机器人 — 多机协同路权分配 + 时间窗避让”可以概括为：上层把走廊、路口、电梯、门口等共享空间抽象成“时空资源”，用时间窗预约通行权；下层由 BLDC + FOC 把调度指令平滑执行，保证送餐过程不洒漏、不碰撞、不死锁。

一、主要特点

1. 路权分配：把“谁先走”变成可计算的优先级规则
   路权不是简单的人为让行，而是基于任务状态、路径冲突和空间资源进行仲裁。常见优先级因素包括：是否载餐、餐品是否易洒、任务紧急度、剩余电量、是否接近冲突点、是否已在主干道、等待时间等。
   高优先级机器人保留原路径，低优先级机器人通过等待、减速、绕行或退到避让点让行。为避免“长期饥饿”，通常还要引入等待时间加权或最大等待超时机制。
2. 时间窗避让：把空间冲突扩展到时间维度
   时间窗方法把物理路网展开成“位置 + 时间”的占用表。每条边、每个路口或每个栅格都记录占用时间段；若两台机器人的占用时间重叠，就视为冲突。
   冲突消解通常有三种方式：
   时间错位：让低优先级机器人延迟出发或短暂等待；
   路径重规划：把被占用的时空格子视为临时障碍，重新搜索路径；
   速度调节：通过减速拉长通过时间，避免同时进入冲突区。
3. 冲突类型需要分层处理
   多机冲突不只是“撞在一起”。典型类型包括：
   顶点冲突：两台机器人同时占据同一路口或栅格；
   边冲突：两台机器人在同一时间相向通过同一段通道；
   跟随冲突：后车紧贴前车，形成追尾风险；
   交换冲突：两台机器人在相邻格子互换位置；
   死锁：多台机器人在窄通道或路口互相等待，无法继续前进。
   时间窗方法能较好处理前几类冲突，但死锁通常需要额外机制，例如超时重规划、避让点、虚拟交通灯或集中式冲突聚类。
4. 集中式与分布式协同各有取舍
   集中式调度由云端或上位机统一管理时间窗和优先级，适合餐厅、酒店、仓储等结构化场景，优点是全局一致性好，缺点是存在单点故障和通信压力。
   分布式协商通过 V2V 通信让机器人自主协商路权，优点是鲁棒性强，缺点是容易出现优先级不一致、通信延迟误判和对称死锁。
   工程上更常见的是混合架构：上位机负责全局时间窗和任务调度，边缘节点或机器人本地负责毫秒级冲突响应和紧急避障。
5. BLDC + FOC 是执行层的关键优势
   送餐机器人对低速平稳性要求很高。BLDC 配合 FOC 可以降低转矩脉动，实现平滑加减速、低速精确转向和柔性停车，减少汤汁洒漏和乘客不适。
   在路权让行场景中，机器人经常需要减速等待、低速微调、原地转向或小角度修正，这些动作对电机低速力矩控制要求较高。FOC 的电流环闭环能力比简单六步换相或普通 ESC 更适合这类协同调度执行。

二、典型应用场景

1. 餐厅/火锅店多机送餐
   通道狭窄、桌椅复杂、人流密集，多台机器人容易在门口、拐角、出餐口相遇。时间窗可以避免多台机器人同时挤入同一通道，路权分配可让载餐或易洒餐品优先通行。
2. 酒店客房配送
   走廊长、电梯口拥堵、地面材质变化多。电梯可视为高竞争资源，需要时间片或时间窗预约；机器人进出电梯时也需明确互斥区，避免卡门或互相阻挡。
3. 医院药品/餐食配送
   对安全、安静和可靠性要求更高。急救或高优先级任务可获得更高路权，普通配送机器人需主动退让到安全区域。
4. 写字楼/园区外卖配送
   电梯厅、闸机口、大堂是典型瓶颈区域。系统需要处理多楼层、多电梯、多机器人并发请求，时间窗和优先级调度价值更大。
5. 教学与科研平台
   可用于验证多机路径规划、时间窗冲突检测、优先级仲裁、死锁消解、V2V 通信以及 BLDC 柔顺执行等完整链路。

三、需要注意的事项

1. 平台算力边界要分层设计
   标准 Arduino Uno 不适合独立运行 SLAM、多机调度、时间窗规划和 FOC。更稳妥的是上位机负责地图、调度和时间窗管理，下位 MCU 负责 BLDC 控制、编码器采集、IMU 读取和紧急避障。
2. 时间窗必须带安全裕量
   机器人实际速度、定位误差、通信延迟和地面打滑都会让理论时间窗偏离实际。因此占用时间窗应加入缓冲，例如在理论通过时间前后增加安全余量，避免“刚好不撞”变成“边缘碰撞”。
3. 优先级规则要全局一致
   如果不同机器人使用不同仲裁逻辑，就可能出现对称死锁。应预先约定统一规则，例如先比较任务等级，再比较载餐状态，再比较等待时间或距冲突点距离。
4. 死锁检测不能只看“速度为零”
   机器人暂停可能是正常等电梯、等行人或等路权释放。更可靠的做法是综合判断：连续多周期位移极小、期望方向被阻挡、邻居同样停滞、冲突区相互占用。
5. 互斥区定义要合理
   互斥区过小会反复进入退出，造成抖动；过大则降低通行效率。应根据机器人尺寸、制动距离、定位误差和传感器精度标定，并在门口、电梯、窄通道等位置单独设置。
6. 通信延迟会影响协同效果
   多机协同依赖位置、速度、意图和时间窗信息交换。若使用 WiFi，应控制通信延迟和丢包；关键指令要有超时保护和保守降级策略。通信丢失时宁可等待，也不要冒险进入冲突区。
7. BLDC 执行层要做好电流限幅和防洒漏
   频繁等待、起步、停车会让电机电流波动增大。应限制加加速度，采用 S 曲线加减速，并对 q 轴电流、最大角速度和退让速度做限幅，避免电压跌落、主控复位或餐品洒漏。
8. 必须有硬件级安全兜底
   算法层的路权和时间窗不能替代本体安全。激光雷达、超声波、 bumper 或急停回路应保持最高优先级；一旦本地传感器检测到近距离障碍，应立即减速或停车，不受上层调度指令覆盖。
   整体而言，这个方案的核心链路是：全局地图与任务调度 → 时空时间窗占用表 → 冲突检测与优先级仲裁 → 本地速度/路径修正 → BLDC 柔顺执行。Arduino 更适合承担底层电机控制与传感器采集，而多机协同、时间窗管理和死锁消解通常需要上位机或更高性能 MCU 来完成。

1、基于任务优先级的瓶颈路段路权分配
此案例展示最基础的多机协同逻辑：两台机器人接近同一窄道时，携带高优先级餐品的一方优先通行，另一方原地等待。

#include <SimpleFOC.h>

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

// 机器人身份与任务优先级
// 优先级：1=普通送餐，2=热菜/急单，3=空载收碗
int myPriority = 1;
int myID = 1;

// 对向机器人数据（ESP-NOW 接收）
struct OpponentData {
    int id;
    int priority;
    float distanceToBottleneck; // 距窄道入口距离(m)
    bool inBottleneck;
};
OpponentData opp = {0, 0, 999.0, false};

// 本机状态
float myDistToBottleneck = 999.0;
bool hasRightOfWay = false;
enum RobotState { MOVING, WAITING, PASSING };
RobotState state = MOVING;

void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len) {
    memcpy(&opp, data, sizeof(opp));
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 1. 检测是否接近窄道
    myDistToBottleneck = readDistanceToBottleneck();
    
    // 2. 若双方均在窄道附近，执行路权仲裁
    if (myDistToBottleneck < 0.5 && opp.distanceToBottleneck < 0.5) {
        arbitrateRightOfWay();
    }
    
    // 3. 根据状态执行运动
    switch (state) {
        case MOVING:
            driveForward(0.3); // 0.3 m/s
            break;
        case WAITING:
            stopMotors();
            break;
        case PASSING:
            driveForward(0.4); // 优先通行时略快
            break;
    }
    
    delay(50);
}

void arbitrateRightOfWay() {
    // 路权规则：高优先级优先；同优先级时，先到者优先
    bool myHigherPriority = (myPriority > opp.priority);
    bool samePriorityAndFirstArrival = (myPriority == opp.priority && 
                                         myDistToBottleneck < opp.distanceToBottleneck);
    
    if (myHigherPriority || samePriorityAndFirstArrival) {
        state = PASSING;
        hasRightOfWay = true;
    } else {
        state = WAITING;
        hasRightOfWay = false;
    }
}

float readDistanceToBottleneck() {
    // 实际项目中通过超声波/激光传感器测距
    // 此处返回模拟值
    return 0.4;
}

路权分配的核心是优先级比较。高优先级任务（如热菜、急单）应获得通行权。同优先级时，距离窄道更近的一方先通行，避免“对向僵持”。

2、时间窗预留与避让点计算
此案例引入时间窗概念：机器人通过通信交换各自到达冲突点的时间窗口，低优先级一方计算避让点并等待，确保时间窗不重叠。

#include <SimpleFOC.h>

// 冲突点坐标
struct ConflictPoint {
    float x, y;
    int conflictID;
};

// 时间窗结构
struct TimeWindow {
    unsigned long startTime;   // 进入冲突区时刻
    unsigned long endTime;     // 离开冲突区时刻
    int robotID;
    int priority;
};

// 本机时间窗
TimeWindow myWindow;

// 避让参数
float avoidDistance = 0.5;     // 避让距离(m)
float robotSpeed = 0.3;        // 平均速度(m/s)
float safetyMargin = 0.3;      // 安全余量(m)

void computeTimeWindow(float distToConflict) {
    unsigned long travelTime = (unsigned long)((distToConflict * 1000) / robotSpeed);
    unsigned long passDuration = (unsigned long)((0.5 * 1000) / robotSpeed); // 通过冲突区耗时
    
    myWindow.startTime = millis() + travelTime;
    myWindow.endTime = myWindow.startTime + passDuration;
    myWindow.robotID = 1;
    myWindow.priority = 1;
}

// 检查时间窗是否与对向冲突
bool checkWindowConflict(TimeWindow other) {
    // 时间窗重叠检测
    if (myWindow.startTime < other.endTime && myWindow.endTime > other.startTime) {
        return true; // 冲突
    }
    return false;
}

// 计算避让等待时长
unsigned long computeAvoidTime(TimeWindow other) {
    if (myWindow.priority > other.priority) {
        return 0; // 高优先级不避让
    }
    
    // 计算需要延迟的时间，使本机时间窗移到对方之后
    // 简化：等待到对方时间窗结束 + 安全余量
    long waitTime = (long)(other.endTime - myWindow.startTime) + 
                    (safetyMargin * 1000 / robotSpeed);
    
    return (waitTime > 0) ? (unsigned long)waitTime : 0;
}

// 执行避让：停在避让点等待
void executeAvoidance(unsigned long waitTime) {
    stopMotors();
    Serial.print("避让等待: ");
    Serial.print(waitTime);
    Serial.println(" ms");
    
    unsigned long startWait = millis();
    while (millis() - startWait < waitTime) {
        // 保持停止，可执行传感器采样
        delay(10);
    }
    Serial.println("避让结束，继续通行");
}

void loop() {
    // 1. 计算本机到冲突点的距离和时间窗
    float distToConflict = readDistanceToConflict();
    computeTimeWindow(distToConflict);
    
    // 2. 接收对向机器人时间窗（通过 ESP-NOW/串口）
    TimeWindow otherWindow = receiveOpponentWindow();
    
    // 3. 冲突检测与避让决策
    if (checkWindowConflict(otherWindow)) {
        unsigned long avoidTime = computeAvoidTime(otherWindow);
        if (avoidTime > 0) {
            executeAvoidance(avoidTime);
        }
    }
    
    // 4. 无冲突则继续通行
    driveForward(0.3);
    delay(50);
}

要点：时间窗的本质是为每个机器人“预留”通过冲突路段的时间段。当两个机器人的时间窗重叠时，低优先级一方计算需要延迟多久，然后停在避让点等待，直到高优先级机器人的时间窗结束。

3、分布式协商与死锁消解
此案例处理更复杂的场景：当多台机器人同时到达同一交叉口且优先级相同时，通过分布式协商避免死锁。

#include <SimpleFOC.h>

// 机器人状态
struct RobotState {
    int id;
    int priority;
    float x, y;
    float speed;
    bool waiting;
    unsigned long waitStartTime;
};

// 本机和对手机器人状态（最多支持 4 台）
RobotState robots[4];
int robotCount = 0;
int myIndex = 0;

// 协商参数
const unsigned long DEADLOCK_TIMEOUT = 3000;  // 3秒死锁检测
const unsigned long BACKOFF_BASE = 500;       // 退避基础时间

// 避让决策
enum Decision { PROCEED, WAIT, BACKOFF };
Decision myDecision = PROCEED;

void loop() {
    // 1. 更新本机状态
    updateMyState();
    
    // 2. 接收其他机器人状态
    receiveRobotStates();
    
    // 3. 检测潜在冲突
    if (detectConflict()) {
        myDecision = negotiatePriority();
    } else {
        myDecision = PROCEED;
    }
    
    // 4. 执行决策
    switch (myDecision) {
        case PROCEED:
            driveForward(0.3);
            resetDeadlockTimer();
            break;
        case WAIT:
            stopMotors();
            if (checkDeadlockTimeout()) {
                // 等待超时，发起退避
                myDecision = BACKOFF;
            }
            break;
        case BACKOFF:
            executeBackoff();
            break;
    }
    
    // 5. 广播本机状态
    broadcastMyState();
    
    delay(50);
}

// 协商优先级：考虑优先级 + 等待时间 + 随机退避
Decision negotiatePriority() {
    int higherPriorityCount = 0;
    int samePriorityCount = 0;
    
    for (int i = 0; i < robotCount; i++) {
        if (i == myIndex) continue;
        
        if (robots[i].priority > robots[myIndex].priority) {
            higherPriorityCount++;
        } else if (robots[i].priority == robots[myIndex].priority) {
            // 同优先级：比较等待时间，等待更久的优先
            if (robots[i].waitStartTime < robots[myIndex].waitStartTime && 
                robots[i].waiting) {
                samePriorityCount++;
            }
        }
    }
    
    // 有更高优先级 → 等待
    if (higherPriorityCount > 0) {
        if (!robots[myIndex].waiting) {
            robots[myIndex].waitStartTime = millis();
            robots[myIndex].waiting = true;
        }
        return WAIT;
    }
    
    // 同优先级且对方等待更久 → 等待
    if (samePriorityCount > 0) {
        if (!robots[myIndex].waiting) {
            robots[myIndex].waitStartTime = millis();
            robots[myIndex].waiting = true;
        }
        return WAIT;
    }
    
    // 本机最高优先级或等待最久 → 通行
    robots[myIndex].waiting = false;
    return PROCEED;
}

// 死锁检测：等待时间超过阈值
bool checkDeadlockTimeout() {
    if (robots[myIndex].waiting && 
        (millis() - robots[myIndex].waitStartTime) > DEADLOCK_TIMEOUT) {
        return true;
    }
    return false;
}

// 退避：随机后退一段距离，打破死锁
void executeBackoff() {
    // 根据机器人ID生成伪随机退避时间
    unsigned long backoffTime = BACKOFF_BASE + (robots[myIndex].id * 137) % 500;
    
    Serial.print("死锁退避: ");
    Serial.print(backoffTime);
    Serial.println(" ms");
    
    // 后退
    motorL.target = -2.0;
    motorR.target = -2.0;
    delay(backoffTime);
    stopMotors();
    
    // 重置等待状态
    robots[myIndex].waiting = false;
    myDecision = PROCEED;
}

要点：多机死锁的根源是循环等待——A 等 B，B 等 C，C 等 A。分布式协商通过比较等待时间和随机退避来打破循环。等待超时的机器人主动后退，释放空间，让其他机器人先通过，从而消解死锁。

要点解读

1. 路权分配的核心是“任务优先级 + 等待时间”双因子
   单纯的“先到先得”在高峰期会导致空载机器人占据窄道。实际系统中，路权判定应综合餐品优先级（热菜 > 普通餐 > 空载）和等待时间（等待过久则提升优先级）。这保证了紧急订单的及时送达，同时避免低优先级机器人被无限期阻塞。

2. 时间窗是比“即时避让”更高效的策略
   即时避让（看到对方才停）会导致机器人频繁启停，餐品泼洒风险高。时间窗机制让机器人在出发前就规划好通过冲突点的时间段，双方通过通信提前协调，实现“不停车通过”或“最少等待”。这是从反应式避让到预测式协调的关键升级。

3. 分布式协商需要明确的“打破平局”规则
   当多台机器人优先级相同时，必须有规则决定谁先走。可用的规则包括：比较等待时间、比较机器人 ID 的伪随机退避、或引入中央调度器仲裁。没有明确的平局规则，系统会在对称冲突中死锁。

4. 死锁消解需要“主动退让”而非“无限等待”
   分布式系统中，纯等待策略可能形成死锁循环。有效的机制是：等待超过阈值后，低优先级一方主动后退一段距离，释放冲突区域，打破循环。后退距离应足够让另一方通过，但不宜过大以免影响自身任务。

5. BLDC 的柔顺启停是路权执行的物理保障
   路权决策的最终效果取决于电机能否平滑执行“等待—启动—通行”的切换。BLDC 配合 FOC 可实现渐进式加速和减速，避免急停急起导致的餐品泼洒。在等待结束时，电机应从零平滑爬升至目标速度，而非阶跃启动。

1、BLDC 差速底盘 + 格点航迹推算 + 交叉口路权互斥
所有协同的前提是每台车都"知道自己精确在哪"。这一版把 SimpleFOC 双轮闭环、格点定位、以及一个超时自释放的路口互斥锁拼成最小可用底座。

#include <SimpleFOC.h>

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

const float WHEEL_D   = 65.0f;    // mm
const float TRACK     = 220.0f;   // mm 轮距
const float TICK_MM   = 3.14159f * WHEEL_D / 1024.0f;

/* ========== 里程计 ========== */
float odomX = 0, odomY = 0, odomTh = 0;
unsigned long tOdom = 0;

void updateOdom() {
  unsigned long now = millis();
  float dt = (now - tOdom) / 1000.0f;
  if (dt <= 0) return;
  tOdom = now;

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

  odomTh += w * dt;
  odomX  += v * cosf(odomTh) * dt;
  odomY  += v * sinf(odomTh) * dt;
}

/* ========== 格点定位（餐厅格子地板，磁条/二维码校正） ========== */
const float CELL = 600.0f;                      // 一格 600mm
void gridCoord(int &gx, int &gy) {
  gx = (int)roundf(odomX / CELL);
  gy = (int)roundf(odomY / CELL);
}

/* ========== 路权互斥：可超时自释放的节点锁 ========== */
#define N_RES   16
#define LOCK_TTL 4000                        // ms 无心跳自动释放

struct ResLock {
  uint8_t  res;
  uint8_t  owner;
  uint32_t stamp;                            // 最后一次心跳
};

ResLock locks[N_RES];
uint8_t myId = 0;                            // 本机 ID

int findLock(uint8_t res) {
  for (int i = 0; i < N_RES; i++)
    if (locks[i].res == res && locks[i].stamp != 0) return i;
  return -1;
}

void grantLock(uint8_t res, uint8_t who) {   // 收到 GRANT 时调用
  int i = findLock(res);
  if (i < 0) { for (i = 0; i < N_RES; i++) if (locks[i].stamp == 0) break; }
  locks[i].res = res; locks[i].owner = who; locks[i].stamp = millis();
}

bool holdsLock(uint8_t res) {
  int i = findLock(res);
  if (i < 0 || locks[i].owner != myId) return false;
  // TTL 检查：持有者长时间无心跳视为离线，强制回收
  if (millis() - locks[i].stamp > LOCK_TTL) { locks[i].stamp = 0; return false; }
  return true;
}

void releaseLock(uint8_t res, uint8_t who) {
  int i = findLock(res);
  if (i >= 0 && locks[i].owner == who) locks[i].stamp = 0;
}

/* ========== 路口通过 ========== */
const float V_CORRIDOR = 300.0f;             // mm/s 走廊巡航
const float W_TURN     = 0.6f;               // rad/s 转弯

bool enterIntersection(uint8_t res) {
  // 1) 广播 REQ（此处以函数占位，实际发 ESP-NOW）
  broadcastRequest(res, myId);
  // 2) 等待授权，超时则放弃并让行
  unsigned long t0 = millis();
  while (millis() - t0 < 1500) {
    pollRadio();                             // 处理 GRANT / DENY
    if (holdsLock(res)) return true;
    delay(5);
  }
  return false;                              // 让行，等待下一轮
}

void setup() {
  Serial.begin(115200);
  eL.init(); eR.init();
  dL.voltage_power_supply = 24; dR.voltage_power_supply = 24;
  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 = 12;
  mL.init(); mR.init(); mL.initFOC(); mR.initFOC();
  myId = readDeviceId();                     // 拨码开关或 EEPROM
  tOdom = millis();
}

void loop() {
  mL.loopFOC(); mR.loopFOC();
  updateOdom();
  delayMicroseconds(500);
}

LOCK_TTL 是这段代码里最重要的一个常量。没有它，一台断线或卡死的车会永久持有路口锁，整个餐厅的通行效率在半小时内就会归零——这是分布式互斥最典型的死锁形态。

5、路径段时间窗生成与冲突检测（避让与重规划）
路权锁只解决"同一时刻谁进路口"，时间窗解决的是"未来一段时间内整条路线是否会撞车"。核心是把路径切成资源序列，每段带一个 [tIn, tOut] 窗口写进共享预约表。

/* ========== 时间窗数据结构 ========== */
struct Reservation {
  uint8_t  res;               // 资源编号：走廊段 / 路口
  uint8_t  owner;
  uint32_t tStart, tEnd;      // 全局毫秒时基
  uint32_t stamp;
  uint8_t  active;
};

#define N_RESV 24
Reservation table[N_RESV];
const uint32_t GUARD_MS = 800;      // 同资源两台车之间的安全间隔

uint32_t nowMs;                     // 由公共时基同步得到
uint8_t  myId;

/* ========== 运动学：受限加速度下的 ETA ==========
   送餐车防洒，限制加速度 aMax，故不能按匀速算 ETA */
const float A_MAX   = 200.0f;       // mm/s^2
const float V_MAX   = 300.0f;       // mm/s
const float A_TURN  = 150.0f;

uint32_t segDuration(float len, float v0, float &vEnd) {
  // 匀加速到 V_MAX 再可能的减速，简化为梯形/三角形
  float dAcc = (V_MAX * V_MAX - v0 * v0) / (2 * A_MAX);
  if (dAcc >= len) {                       // 三角形速度剖面
    float vPeak = sqrtf(v0 * v0 + 2 * A_MAX * len / 2);
    float t = 2 * (vPeak - v0) / A_MAX;
    vEnd = v0;
    return (uint32_t)(t * 1000);
  }
  float tAcc = (V_MAX - v0) / A_MAX;
  float dDec = V_MAX * V_MAX / (2 * A_MAX);
  float tCru = (len - dAcc - dDec) / V_MAX;
  float tDec = V_MAX / A_MAX;
  vEnd = 0;
  return (uint32_t)((tAcc + tCru + tDec) * 1000);
}

/* ========== 冲突检测：时空重叠判定 ========== */
bool windowConflict(uint8_t res, uint32_t s, uint32_t e) {
  for (int i = 0; i < N_RESV; i++) {
    if (!table[i].active) continue;
    if (table[i].res != res) continue;
    if (table[i].owner == myId) continue;
    // 区间重叠 + 安全间隔
    if (s < table[i].tEnd + GUARD_MS && e + GUARD_MS > table[i].tStart)
      return true;
  }
  return false;
}

/* ========== 为本机路径申请时间窗 ========== */
struct PathSeg { uint8_t res; float len; };

#define N_SEG 8
PathSeg  myPath[N_SEG];
uint8_t  nSeg;
uint32_t tCursor;                   // 本机的"出发时刻"游标

int reservePath(uint32_t tDepart) {
  tCursor = tDepart;
  float v = 0;

  for (int i = 0; i < nSeg; i++) {
    uint32_t dur = segDuration(myPath[i].len, v, v);
    uint32_t s = tCursor, e = tCursor + dur;

    // 冲突 → 顺延本段及其后续，直到不冲突（最多顺延 8 轮）
    int retry = 0;
    while (windowConflict(myPath[i].res, s, e) && retry++ < 8) {
      uint32_t shift = nextFreeGap(myPath[i].res, e);   // 查表求最近空档
      s += shift; e += shift;
    }
    if (retry >= 8) return -1;                          // 无法避让，需换路

    if (!writeReservation(myPath[i].res, myId, s, e)) return -2;
    tCursor = e;
  }
  return tCursor - tDepart;           // 成功，返回总耗时
}

/* ========== 重规划：本段换一条替代路线 ========== */
bool replanPath() {
  // 调用 A*/Dijkstra，把已被占用的资源在当前时间窗内设为高代价边
  for (int i = 0; i < N_RESV; i++)
    if (table[i].active && table[i].owner != myId)
      markResourceCost(table[i].res, 1000 - 0);   // 权重惩罚
  uint8_t len = astar(myPath, &nSeg, N_SEG);
  return len > 0;
}

/* ========== 执行：按时间窗节拍走 ========== */
void loop() {
  updateClockFromRadio();             // 同步全局时基
  nowMs = getGlobalMs();

  static int idx = 0;
  if (idx < nSeg) {
    uint32_t tStart = getReservationStart(myPath[idx].res, myId);
    if (nowMs >= tStart && nowMs < tStart + 300) {
      runSegment(myPath[idx]);        // 沿本段走廊直行 / 路口转向
      idx++;
    } else {
      holdPosition();                 // 未到窗口，就地等待（让行）
    }
  } else {
    idx = 0;
    uint32_t e = reservePath(nowMs + 500);
    if (e < 0) replanPath();          // 预约失败 → 重规划
  }
  delay(10);
}

segDuration() 里之所以要建速度剖面而不是 len / V_MAX，是因为送餐场景下 A_MAX 被"防洒"约束压得很低，加减速段在短走廊里占了大头。用匀速估 ETA 会系统性提前，导致时间窗被系统性低估，最终表现为两台车在路口"擦肩"。

6、多机协同全流程 —— ESP-NOW 广播 + 仲裁 + 死锁检测
两台以上就必须有通信和角色分工。这一版用 ESP-NOW 做状态广播，采用"请求-授权-心跳"三段式，并加入等待图成环检测来破解死锁。

#include <esp_now.h>
#include <WiFi.h>
#include <SimpleFOC.h>

/* ================= 报文定义 ================= */
enum MsgType : uint8_t { MSG_STATE, MSG_REQ, MSG_GRANT, MSG_DENY, MSG_RELEASE };

typedef struct __attribute__((packed)) {
  uint8_t  type;
  uint8_t  src;
  uint8_t  res;            // 资源编号
  uint8_t  prio;           // 优先级：任务紧急度 + 等待时长
  uint32_t tStamp;
  uint16_t gx, gy;         // 格点位置，用于几何消解
  uint32_t tStart, tEnd;   // 时间窗
} Packet;

uint8_t broadcastAddr[] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
uint8_t myId;
Packet  rx, tx;

/* ================= 路权表与等待图 ================= */
#define N_PEER  6
#define N_RES   16

struct LockEntry { uint8_t res, owner; uint32_t stamp; };
LockEntry locks[N_RES];

uint8_t waitFor[N_PEER];        // waitFor[me] 这类：我在等谁让路
uint32_t reqTime[N_RES];        // 我发起请求的时刻

/* ================= 优先级计算 ================= */
const float W_WAIT = 0.5f;
uint8_t myPrio() {
  uint32_t waitMs = millis() - earliestReqTime();
  return (uint8_t)constrain(baseTaskPriority() + waitMs * W_WAIT / 1000.0f, 0, 250);
}

/* ================= 死锁检测：等待图找环 ================= */
bool wouldDeadlock(uint8_t from, uint8_t to) {
  // 若 to 已经在等 from（或链条回到 from），Grant 会成环
  uint8_t p = to;
  for (int i = 0; i < N_PEER; i++) {
    if (waitFor[p] == 0xFF) return false;
    if (waitFor[p] == from) return true;
    p = waitFor[p];
  }
  return false;
}

/* ================= 收到报文 ================= */
void onRecv(const uint8_t *mac, const uint8_t *data, int len) {
  memcpy(&rx, data, sizeof(rx));

  switch (rx.type) {
    case MSG_REQ:
      // 资源空闲 或 请求者优先级更高 → 让权
      if (!holdsLock(rx.res, myId)) {
        if (rx.prio > myPrio() && !wouldDeadlock(rx.src, myId)) {
          sendAck(MSG_GRANT, rx.res, rx.src);
        } else {
          sendAck(MSG_DENY, rx.res, rx.src);
        }
      } else if (rx.prio > myPrio() + 40) {
        // 显著更高优先级：主动释放并让行（防活锁的关键）
        releaseLock(rx.res, myId);
        sendAck(MSG_GRANT, rx.res, rx.src);
      } else {
        sendAck(MSG_DENY, rx.res, rx.src);
      }
      break;

    case MSG_GRANT:
      if (rx.src == myId) {
        setLock(rx.res, rx.src);
        waitFor[myId] = 0xFF;             // 解除等待状态
      }
      break;

    case MSG_DENY:
      if (rx.src == myId) waitFor[myId] = rx.owner;   // 记录在等谁
      break;

    case MSG_RELEASE:
      clearLock(rx.res, rx.src);
      break;

    case MSG_STATE:
      // 邻居状态：更新其位置用于几何预测（做次级避让）
      updatePeerPose(rx.src, rx.gx, rx.gy, rx.tStamp);
      break;
  }
}

/* ================= 心跳：持有资源则周期广播 ================= */
void heartbeat() {
  static uint32_t tHb = 0;
  if (millis() - tHb < 500) return;
  tHb = millis();
  for (int i = 0; i < N_RES; i++) {
    if (locks[i].res != 0xFF && locks[i].owner == myId) {
      tx.type = MSG_STATE; tx.src = myId; tx.res = locks[i].res;
      tx.prio = myPrio(); tx.tStamp = millis();
      esp_now_send(broadcastAddr, (uint8_t*)&tx, sizeof(tx));
    }
  }
}

/* ================= 走廊会车：几何 + 时间窗双层避让 ================= */
void corridorYield() {
  int gx, gy; gridCoord(gx, gy);
  for (int p = 0; p < N_PEER; p++) {
    if (p == myId || !peerAlive(p)) continue;

    int px, py; getPeerCell(p, px, py);
    int dx = px - gx, dy = py - gy;
    int manh = abs(dx) + abs(dy);

    if (manh <= 2) {                       // 两格以内才需要博弈
      bool headOn = isHeadOn(p);           // 朝向相对判定
      uint8_t pw = peerWaitTime(p);
      if (headOn && (pw > myWaitTime() || myPrio() < peerPrio(p))) {
        // 对方等得久 → 本机退入待避点
        pullToSiding();
        return;
      }
      if (headOn) {
        // 本机优先：向对方广播本段的通行时间窗，请对方让行
        tx.type = MSG_REQ; tx.src = myId; tx.res = segmentOf(gx, gy);
        tx.prio = myPrio(); tx.tStamp = millis();
        tx.tStart = millis() + 300;
        tx.tEnd   = tx.tStart + estimateSegmentMs(gx, gy, px, py);
        esp_now_send(broadcastAddr, (uint8_t*)&tx, sizeof(tx));
      }
    }
  }
}

/* ================= 主循环 ================= */
void loop() {
  mL.loopFOC(); mR.loopFOC();
  updateOdom();

  heartbeat();                 // 1) 维持路权心跳
  checkExpiredLocks();         // 2) 回收超时锁（防死锁兜底）
  checkDeadlockChain();        // 3) 等待图成环 → 强制退让
  corridorYield();             // 4) 走廊会车博弈
  followTimeWindows();         // 5) 按预约窗行驶

  delay(5);
}

三处设计值得注意：heartbeat() 让"持有权"有生命期，checkExpiredLocks() 是死锁的兜底回收，wouldDeadlock() 在授权前预判成环——这三者共同保证系统在最坏情况下仍会自行解开，而不是原地僵住。

要点解读
第一，调度层的 ETA 精度完全由执行层决定，而送餐场景必须用受加速度约束的速度剖面。 BLDC 速度环的带宽足够快，但"防洒"要求把加速度压到 0.1–0.2 g 量级，短走廊里加减速段占比过半，匀速估算会系统性偏乐观。时间窗一旦建立在乐观 ETA 上，两台车会在路口以"各自认为自己准时"的状态相遇——这是送餐机器人协同里最隐蔽的失效模式。所以 A_MAX、A_TURN、V_MAX 和轮径一样，属于必须现场标定的常量；载重变化时还应按负载查表调整。

第二，路权分配有三种范式，选错会导致效率或安全的极端失衡。 令牌传递最安全但吞吐低，适合窄门、单梯等强互斥点；预约式（时间窗）吞吐高、可提前规划，适合走廊与常规路口；优先级抢占响应快，但必须配年龄老化（aging）防止低优先级车永久饥饿。工程上通常把三者串起来用：走廊用预约、路口用令牌、死锁时用优先级抢占。myPrio() 把等待时长直接计入优先级，就是最廉价的防饥饿手段。

第三，时间窗避让检测的是"资源 × 时间"的二维重叠，资源粒度是整套系统的核心设计变量。 粒度取整条主走廊，安全但会把并发度压到只有一台车通行；粒度取每米路段的细段，效率高但必须额外处理迎面会车与追尾。案例二的 GUARD_MS 是把"粒度不足"转为"安全冗余"的手段——窗口不重叠还不够，还要留出物理制动距离对应的时间。粒度、安全间隔、以及是否允许单向走廊，这三个参数要一起定。

第四，任何依赖无线通信的协同系统都必须默认"通信会失效"。 ESP-NOW 有物理层重传但没有应用层可靠交付，报文会丢、会乱序、会假离线。所以要有三件套：状态心跳让持有权具备生命期、TTL 到期自动回收、等待图成环时由低优先级方强制退让。更进一步，仲裁逻辑不能只有单一调度者——中央调度器一旦掉线全场瘫痪。务实做法是"中央调度为常态、去中心化为兜底"：正常情况下由调度器统一排窗，失联后各车自动降级到案例一的本地令牌模式。

第五，餐厅场景的动态障碍与物理约束必须显式建模，否则再好的调度也是空转。 顾客、服务员的移动是高频随机扰动，无法纳入预约表，只能靠车端近距传感器做反应式避让，而这会打乱时间窗——因此调度层必须留出"扰动裕度"，并在检测到长时间停滞时把该车已占窗口标为待失效、通知后车重算。电梯、窄门、取餐台口是天然的单向瓶颈点，应设为强互斥资源并配独立的超时策略。此外，载重直接影响制动距离与转弯侧倾：满载时制动力需要上调、过弯速度需要下调，这些参数最好由称重传感器或电流估算在线修正，而不是写死在代码里。归根到底，这套系统的鲁棒性来自"乐观规划 + 悲观兜底"的双层结构——上层用时间窗追求通行效率，下层用互斥锁与超时回收保证绝不互撞、绝不僵死。

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