Arduino BLDC仓储协作方案是以Arduino/ESP32为控制核心、BLDC无刷电机为执行器，通过多传感器融合定位与状态机任务调度，实现机器人对操作工人的智能跟随与多任务按需切换的人机协同系统。 该方案具备多模态感知与高动态响应、选择性跟随与多机协同、任务状态机驱动按需切换、避障优先与容错降级四大特点，主要应用于智能仓储拣选、车间物料配送、多机协同调度及科研教学等场景；实际部署时需重点关注主控算力与实时性、电源隔离与EMC、传感器融合精度、任务切换平滑性及安全机制。
一、 技术架构与主要特点
多模态感知与高动态响应：系统通常采用UWB/视觉/IMU/轮式里程计等多传感器融合方案进行目标定位与跟踪。底层BLDC电机配合FOC（磁场定向控制）算法，具备毫秒级扭矩响应能力，能迅速平滑地执行差速转向指令，确保在目标突然启停或急转弯时"如影随形"。
选择性跟随与多机协同：通过为不同UWB标签或视觉目标分配独立ID，机器人能在多人作业环境中精准锁定并跟随指定操作员，避免多机协同时的"串台"与路径冲突。结合动态任务分配算法，系统可根据距离、当前任务负载、剩余电量等维度实时指派最优跟随任务。
任务状态机驱动的按需切换：系统内置有限状态机（FSM），定义"跟随"“搬运”“充电”“待命”"避障"等离散状态。当工人发出取货指令或到达指定货架时，机器人自动从跟随模式切换至搬运模式；任务完成后自动回归跟随状态。状态切换由上位机指令或本地传感器事件触发，Arduino端仅执行状态转移逻辑与电机控制。
避障优先与容错降级：遵循"跟随为任务、避障为生存"原则。当超声波或ToF传感器检测到前方障碍物时，立即挂起跟随任务执行避让；当视觉或UWB信号短暂丢失时，无缝切换至基于IMU和轮式里程计的惯性航位推算模式维持短暂跟随，信号恢复后自动切回主模式。
二、 典型应用场景
智能仓储人机协同拣选：在大型仓库中，多名拣货员同时作业，多台搭载BLDC底盘的AGV自动识别并跟随各自的拣货员。差分定位确保机器人在狭窄货架通道中精准保持与工人的相对方位，承载拣选篮或重物，大幅提升物流流转效率。
车间物料与工具动态配送：在制造车间，机器人可根据工位呼叫或跟随特定操作员，将物料、重型工具精准配送至指定位置。多任务切换机制使其能在"跟随配送"与"自主返回取货点"之间按需切换，减轻工人搬运负担。
多机器人集群协同调度：多台Arduino控制的机器人通过总线或无线通信共享状态信息。当某台机器人电量低或故障时，调度系统自动将其任务转移给闲置机器人，实现全局效率最大化。
教学与原型验证平台：成本远低于商用伺服系统，适合高校和职业院校用于机器人运动控制、多传感器融合、状态机设计等教学实训，也可用于RoboMaster等机器人竞赛中的跟随与协同算法验证。
三、 关键注意事项
主控算力与实时性保障：多传感器融合、FOC控制与状态机逻辑对算力要求较高。标准Arduino Uno（16MHz）难以胜任，建议采用ESP32（双核240MHz）或STM32等高算力板卡。控制回路必须使用硬件定时器中断或非阻塞定时（millis()），严禁使用delay()函数，确保控制频率≥50Hz。
电源隔离与电磁兼容（EMC）：BLDC电机启停时电流冲击极大，严禁与Arduino及传感器共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容吸收反电动势。动力线与信号线必须分开走线，IMU需使用屏蔽线并远离电机。
传感器融合精度与容错：UWB在极端遮挡下可能出现测距跳变，必须引入IMU与轮式里程计通过扩展卡尔曼滤波（EKF）进行紧耦合融合。视觉跟随需设置合理的置信度阈值，过滤低置信度检测结果，避免误将墙壁或背景识别为跟随目标。
任务切换的平滑性：状态切换时需对电机速度做缓变处理（梯形速度曲线），避免从高速跟随瞬间切换至静止搬运时产生机械冲击。建议在状态机中设置"减速过渡"中间态，确保切换过程平滑。
安全机制必须完善：
通讯超时保护：设定超时阈值（如5秒无心跳包则自动停机），防止信号丢失后机器人失控。
硬件急停：通过急停按钮直接切断电机驱动电源，不依赖软件响应。
速度/加速度限幅：在软件中对目标值做constrain()限幅，避免急启急停对机械结构的冲击。
断点续传：长任务中断后（如低电量充电），需保存当前任务进度，恢复后从断点继续而非从头开始。
IMU安装与抗振设计：IMU必须刚性固定在底盘重心附近，并加硅胶减震垫以隔离BLDC电机的高频振动，否则振动噪声会严重干扰姿态解算精度。

1、双UWB差分定位 + 多目标动态跟随与切换
适用场景：大型仓库中，机器人需在多个佩戴UWB标签的拣货员之间切换跟随，同时避开货架障碍物。

核心逻辑：通过双标签UWB差分定位获取目标人员的绝对坐标与航向角，结合卡尔曼滤波平滑定位跳变。串口指令触发跟随目标切换，BLDC FOC执行差速闭环运动。

#include <SimpleFOC.h>
#include <DWM1001.h>      // UWB定位库
#include <Kalman.h>       // 卡尔曼滤波

// ==================== BLDC差速底盘 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// ==================== UWB多目标追踪 ====================
#define MAX_TARGETS 3
struct Target {
    float x, y;
    uint8_t id;           // 目标ID，用于选择性跟随
    bool active;
};
Target targets[MAX_TARGETS];
int currentTargetID = 1;   // 默认跟随目标1
float robotX = 0, robotY = 0;

// 卡尔曼滤波器（平滑UWB跳变）
KalmanFilter kfX(0.01, 0.1), kfY(0.01, 0.1);

// ==================== 跟随参数 ====================
const float FOLLOW_DIST = 1.2;      // 期望跟随距离(m)
const float MAX_SPEED = 0.8;
const float SAFE_DIST = 0.3;        // 避障触发距离(m)

// ==================== 串口指令解析 ====================
String receivedCmd = "";
bool cmdProcessed = false;

void setup() {
    Serial.begin(115200);
    // 初始化BLDC电机与FOC（略）
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    
    // 初始化UWB模块
    // dwm.init(DWM1001_CS, DWM1001_IRQ, DWM1001_RST);
    
    // 预设目标坐标（模拟）
    targets[0] = {2.0, 1.0, 1, true};
    targets[1] = {5.0, 3.0, 2, true};
    targets[2] = {8.0, 6.0, 3, true};
}

// ==================== 串口命令处理（目标切换） ====================
void processCommand(String cmd) {
    if (cmd.startsWith("F")) {
        int id = cmd.substring(1).toInt();
        if (id >= 1 && id <= MAX_TARGETS && targets[id-1].active) {
            currentTargetID = id;
            Serial.print("Switched to target: ");
            Serial.println(id);
        }
    } else if (cmd == "STOP") {
        motorL.move(0); motorR.move(0);
    }
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 解析串口指令（上位机/远程切换目标）
    if (Serial.available() > 0) {
        receivedCmd = Serial.readStringUntil('\n');
        receivedCmd.trim();
        processCommand(receivedCmd);
    }
    
    // 2. UWB定位更新（实际从模块读取）
    // 模拟目标运动
    static float phase = 0;
    phase += 0.02;
    targets[0].x = 2.0 + 0.5 * sin(phase);
    targets[0].y = 1.0 + 0.3 * cos(phase * 0.7);
    
    // 3. 获取当前目标坐标
    Target* currentTarget = nullptr;
    for (int i = 0; i < MAX_TARGETS; i++) {
        if (targets[i].id == currentTargetID && targets[i].active) {
            currentTarget = &targets[i];
            break;
        }
    }
    if (!currentTarget) {
        motorL.move(0); motorR.move(0);
        delay(50);
        return;
    }
    
    // 4. 计算与目标的相对位置
    float dx = currentTarget->x - robotX;
    float dy = currentTarget->y - robotY;
    float dist = sqrt(dx*dx + dy*dy);
    
    // 5. 避障优先检测（超声波/红外）
    // int frontDist = sonarFront.ping_cm();
    // if (frontDist > 0 && frontDist < SAFE_DIST * 100) { 急停逻辑 }
    
    // 6. 跟随控制
    float speed = constrain((dist - FOLLOW_DIST) * 0.3, 0.1, MAX_SPEED);
    float angle = atan2(dy, dx);
    float wheelBase = 0.25;
    motorL.move(speed - angle * wheelBase / 2);
    motorR.move(speed + angle * wheelBase / 2);
    
    delay(50);
}

2、多任务分配 + 轨迹优先级仲裁（双机器人协同补货）
适用场景：两台AGV在仓储环境中协同补货，一台负责拣货，一台负责补货；需动态分配任务并避免路径冲突。

核心逻辑：主机分配任务并规划路径，从机接收任务后通过UWB获取自身与目标位置，执行BLDC闭环运动。采用优先级仲裁和时间窗分配避免交叉冲突。

#include <SimpleFOC.h>
#include <DWM1001.h>

// ==================== BLDC差速底盘 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// ==================== 任务结构 ====================
struct Task {
    int id;
    float targetX, targetY;
    int priority;         // 1高，2中，3低
    bool assigned;
};

Task taskQueue[5] = {
    {1, 3.0, 2.0, 1, false},
    {2, 6.0, 4.0, 2, false},
    {3, 1.0, 5.0, 3, false}
};
int currentTaskIdx = -1;

// ==================== 机器人状态 ====================
float robotX = 0, robotY = 0;
float neighborX = 0, neighborY = 0;  // 另一台机器人位置

// ==================== 任务分配函数 ====================
int allocateTask() {
    int bestIdx = -1;
    float bestScore = -999;
    for (int i = 0; i < 5; i++) {
        if (taskQueue[i].assigned) continue;
        float dx = taskQueue[i].targetX - robotX;
        float dy = taskQueue[i].targetY - robotY;
        float dist = sqrt(dx*dx + dy*dy);
        // 优先级权重 - 距离成本
        float score = (4 - taskQueue[i].priority) * 2.0 - dist * 0.5;
        if (score > bestScore) {
            bestScore = score;
            bestIdx = i;
        }
    }
    return bestIdx;
}

// ==================== 避碰仲裁（优先级+时间窗） ====================
bool checkCollisionRisk(float targetX, float targetY) {
    float dx = neighborX - robotX;
    float dy = neighborY - robotY;
    float dist = sqrt(dx*dx + dy*dy);
    // 若邻居距离小于安全阈值且双方目标方向交叉，触发避碰
    return dist < 1.0;
}

void setup() {
    Serial.begin(115200);
    // 初始化BLDC电机与FOC（略）
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    
    // 初始化UWB（略）
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 更新自身位置（UWB）
    // robotX, robotY 从UWB模块获取
    
    // 2. 任务分配（任务完成后自动分配下一个）
    if (currentTaskIdx == -1) {
        currentTaskIdx = allocateTask();
        if (currentTaskIdx >= 0) {
            taskQueue[currentTaskIdx].assigned = true;
            Serial.print("Assigned task: "); Serial.println(currentTaskIdx);
        }
    }
    
    // 3. 执行任务
    if (currentTaskIdx >= 0) {
        float dx = taskQueue[currentTaskIdx].targetX - robotX;
        float dy = taskQueue[currentTaskIdx].targetY - robotY;
        float dist = sqrt(dx*dx + dy*dy);
        
        // 到达目标，标记完成
        if (dist < 0.2) {
            currentTaskIdx = -1;
            motorL.move(0); motorR.move(0);
            delay(50);
            return;
        }
        
        // 避碰检查：若邻居在附近且路径交叉，等待或绕行
        if (checkCollisionRisk(taskQueue[currentTaskIdx].targetX, 
                                taskQueue[currentTaskIdx].targetY)) {
            motorL.move(0); motorR.move(0);
            Serial.println("Collision risk, waiting...");
            delay(200);
            return;
        }
        
        // 前往目标
        float speed = constrain(dist * 0.4, 0.1, 0.7);
        float angle = atan2(dy, dx);
        float wheelBase = 0.25;
        motorL.move(speed - angle * wheelBase / 2);
        motorR.move(speed + angle * wheelBase / 2);
    }
    
    delay(50);
}

3、集中式调度 + BLDC远程速度指令控制
适用场景：上位机（工业PC/PLC）集中监控多台AGV，通过串口下发速度指令，机器人执行BLDC闭环运动并反馈状态。

核心逻辑：Arduino作为执行器接收串口指令，解析速度/转向值后通过PID控制BLDC电机，同时上报编码器反馈和故障状态。

#include <SimpleFOC.h>

// ==================== BLDC差速底盘 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// ==================== 指令解析 ====================
String inputBuffer = "";
float targetSpeedL = 0, targetSpeedR = 0;
bool newCmd = false;

void setup() {
    Serial.begin(115200);
    
    // 初始化BLDC电机与FOC
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 解析串口指令（格式: V左速,右速）
    while (Serial.available()) {
        char c = Serial.read();
        if (c == '\n') {
            // 解析 "V,0.5,-0.3" 格式
            int idx1 = inputBuffer.indexOf(',');
            int idx2 = inputBuffer.indexOf(',', idx1 + 1);
            if (idx1 > 0 && idx2 > idx1) {
                targetSpeedL = inputBuffer.substring(idx1 + 1, idx2).toFloat();
                targetSpeedR = inputBuffer.substring(idx2 + 1).toFloat();
                newCmd = true;
            }
            inputBuffer = "";
        } else {
            inputBuffer += c;
        }
    }
    
    // 2. 执行速度指令
    if (newCmd) {
        motorL.move(targetSpeedL);
        motorR.move(targetSpeedR);
        newCmd = false;
    }
    
    // 3. 反馈状态（速度、编码器、故障）
    static unsigned long lastFeedback = 0;
    if (millis() - lastFeedback > 100) {
        Serial.print("S,");
        Serial.print(motorL.shaft_velocity); Serial.print(",");
        Serial.print(motorR.shaft_velocity); Serial.print(",");
        Serial.print(encoderL.getCount()); Serial.print(",");
        Serial.println(encoderR.getCount());
        lastFeedback = millis();
    }
    
    delay(20);
}

要点解读

1. 双UWB差分定位解决“仅测距不测向”的核心痛点：单个UWB标签只能获得目标距离，无法判断朝向。双标签差分定位通过计算两个标签的连线向量，直接解算出目标的航向角，使跟随机器人具备“预判”能力，转向时能提前切入跟随曲线而非机械追尾。在金属货架密集的仓库中，UWB的抗多径干扰能力优于WiFi/蓝牙方案。

2. 任务分配的“优先级权重-距离成本”是实现柔性调度的关键：案例二中的任务分配算法综合考虑任务优先级与距离成本，机器人动态选择最高分任务执行。在双机协同场景中，可配合“任务窃取”（Work Stealing）机制——当一台机器人任务受阻时，另一台可主动接管该任务。

3. 避障逻辑拥有硬优先级，独立于跟随/任务：仓储场景中，货架间距狭窄（通常0.8~1.2m），避障是生存底线。当超声波/激光雷达检测到前方距离小于安全阈值（如30cm）时，必须无条件中断跟随或任务，执行急停或避让。这一判断应在底层实时响应，不依赖高层调度。

4. 多目标选择性跟随通过“网络ID锁定”实现防串台：在多人协同作业场景中，机器人需通过分配独立网络ID精准锁定指定操作员，避免多机协同时的“串台”与混乱。案例一中，串口指令F1/F2触发目标切换，实现“指令式”多目标切换。

5. BLDC FOC是“柔顺跟随”的物理执行保障：人机协同要求机器人起步、停止和转向极其柔顺，避免货物倾倒或编队震荡。BLDC配合FOC（磁场定向控制）可实现毫秒级扭矩响应和低速平稳运行，确保每次定位更新的速度指令被平滑执行。上位机+Arduino的分层架构（上位机解算坐标，Arduino专职FOC控制）是应对算力瓶颈的工程标准方案。

4、人机基础动态跟随 + 避障（多任务触发前置）
适用场景：仓储拣货环节，机器人需跟随拣货员移动，同时规避通道临时障碍物（如推车、散落货物），当拣货员触发 “取货任务” 按钮时，自动切换至取货任务准备状态，适用于拣货员与机器人的协作跟随场景。

核心逻辑：
人机跟随策略：通过超声波传感器实时检测人员距离，采用 PID 算法闭环控制 BLDC 电机速度，保持安全跟随距离（0.5-1.2米），人员加速机器人加速，人员减速机器人减速；
动态避障机制：当跟随过程中检测到前方障碍物，触发避障状态，采用 “减速→侧移→重新跟随” 逻辑，避障后自动回归跟随状态；
多任务触发接口：预留任务触发引脚（按钮/RFID），当人员按下取货按钮，立即切换任务状态，暂停跟随进入任务准备，为后续多任务切换奠定基础。

#include <SimpleFOC.h>

// 硬件配置：BLDC差速底盘 + 前方/侧方超声波 + 任务触发按钮
BLDCMotor motorL(9), motorR(10);
BLDCDriver3PWM drvL(3,5,6), drvR(11,12,13);
Encoder encL(18,19,2048), encR(20,21,2048);

const int frontSensor = 2;   // 前方超声波（距离检测）
const int sideSensor = 3;    // 侧方超声波（避障辅助）
const int taskButton = 4;    // 任务触发按钮（人员按下触发取货任务）

// FSM状态定义
enum FsmState {
  STATE_FOLLOW = 0,     // 跟随状态
  STATE_AVOID = 1,      // 避障状态
  STATE_TASK_READY = 2  // 任务准备状态
};
FsmState currentState = STATE_FOLLOW;

// 跟随参数
float followDist = 0.8;   // 目标跟随距离（米）
float pidP = 0.5, pidI = 0.1, pidD = 0.05;
float error = 0, prevError = 0, integral = 0;
float followSpeed = 0.3;

// 避障参数
float obstacleDist = 0.0;
float safeDist = 0.4;

void setup() {
  Serial.begin(115200);
  // 初始化BLDC电机
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 初始化传感器引脚
  pinMode(frontSensor, INPUT);
  pinMode(sideSensor, INPUT);
  pinMode(taskButton, INPUT_PULLUP);
}

void loop() {
  // 读取传感器数据
  obstacleDist = getUltrasonicDist(frontSensor);
  float sideDist = getUltrasonicDist(sideSensor);
  
  // FSM状态切换
  switch (currentState) {
    case STATE_FOLLOW:
      // 避障触发：前方障碍物小于安全距离
      if (obstacleDist < safeDist) {
        currentState = STATE_AVOID;
        Serial.println("Switch to avoidance state");
        motorL.move(0); motorR.move(0);
      }
      // 任务触发：按钮按下
      else if (digitalRead(taskButton) == LOW) {
        currentState = STATE_TASK_READY;
        Serial.println("Task triggered, prepare for pickup");
        motorL.move(0); motorR.move(0);
        delay(300); // 消抖
      }
      else {
        // PID跟随控制
        error = followDist - obstacleDist;
        integral += error * 0.01;
        float derivative = (error - prevError) / 0.01;
        followSpeed = pidP * error + pidI * integral + pidD * derivative;
        // 速度限幅
        followSpeed = constrain(followSpeed, -0.5, 0.5);
        // 差速调整方向（保持与人员对齐）
        float diff = sideDist - followDist;
        float velL = followSpeed - diff * 0.2;
        float velR = followSpeed + diff * 0.2;
        motorL.move(velL); motorR.move(velR);
        prevError = error;
      }
      break;
      
    case STATE_AVOID:
      // 避障逻辑：侧移绕过障碍物
      if (obstacleDist < safeDist) {
        // 右侧无障碍则右转，否则左转
        if (getUltrasonicDist(sideSensor) > safeDist * 1.5) {
          motorL.move(0.3); motorR.move(-0.3);
          delay(500);
        } else {
          motorL.move(-0.3); motorR.move(0.3);
          delay(500);
        }
      } else {
        // 避障完成，返回跟随状态
        currentState = STATE_FOLLOW;
        Serial.println("Avoidance complete, back to follow");
        integral = 0; prevError = 0; // 重置PID
      }
      break;
      
    case STATE_TASK_READY:
      // 任务准备状态：等待下一步任务指令（如货物位置识别）
      // 此处预留接口，后续可扩展货物识别逻辑
      Serial.println("Waiting for task instruction...");
      // 若按钮再次按下，返回跟随状态
      if (digitalRead(taskButton) == HIGH) {
        currentState = STATE_FOLLOW;
      }
      break;
  }
  
  // 电机闭环控制
  motorL.loopFOC();
  motorR.loopFOC();
  delay(10);
}

// 超声波测距函数
float getUltrasonicDist(int pin) {
  int trig = pin + 10; // 假设同引脚模块（简化引脚分配，实际需对应Trig/Echo）
  pinMode(trig, OUTPUT);
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  float dist = pulseIn(pin, HIGH) * 0.034 / 2;
  return constrain(dist, 0.1, 3.0); // 有效距离范围
}

5、目标识别触发跟随与多任务切换
适用场景：仓储货物分拣场景，机器人需识别特定货物标签（RFID/二维码），当识别到目标货物时，自动切换至 “取货跟随” 状态，跟随货物搬运员至货架，取货完成后返回原路径继续等待下一个任务，适用于分拣员与机器人的动态任务协作。

核心逻辑：
目标识别触发：通过 RFID 模块识别货物标签，匹配预设任务（如取货、放货），识别成功立即触发任务切换；
多任务状态管理：设置 “等待→取货跟随→取货执行→返回等待” 四类状态，跟随时结合人机跟随逻辑，取货时执行电机精准定位，完成后自动回归等待；
路径记忆与回归：通过转向计数记录取货路径，返回时按计数反向运动，确保精准回归原路径，避免因路径丢失导致任务中断。

#include <SimpleFOC.h>
#include <SPI.h>
#include <MFRC522.h>

// 硬件配置：BLDC底盘 + RFID（识别货物标签）
BLDCMotor motorL(9), motorR(10);
BLDCDriver3PWM drvL(3,5,6), drvR(11,12,13);
Encoder encL(18,19,2048), encR(20,21,2048);

// RFID引脚
const int ssPin = 10, rstPin = 9;
MFRC522 mfrc522(ssPin, rstPin);

// FSM状态：多任务切换核心
enum FsmState {
  STATE_WAIT = 0,       // 等待任务
  STATE_FOLLOW_TARGET = 1, // 跟随目标（人员/货物）
  STATE_PICK_EXECUTE = 2,   // 取货执行
  STATE_RETURN = 3          // 返回等待
};
FsmState currentState = STATE_WAIT;

// 任务参数
String targetTag = "TASK_001"; // 预设目标标签（货物标识）
int turnCount = 0;            // 转向计数（路径记忆）
int baseTurnCount = 0;        // 记录初始转向计数

// 跟随参数
float targetDist = 0.6;
float followSpeed = 0.3;

void setup() {
  Serial.begin(115200);
  SPI.begin();
  mfrc522.PCD_Init();
  // 初始化电机
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  // RFID识别检测
  if (mfrc522.PICC_IsNewCardPresent() && mfrc522.PICC_ReadCardSerial()) {
    String tag = getTagUid(mfrc522);
    if (tag == targetTag) {
      baseTurnCount = turnCount; // 记录当前路径转向计数
      Serial.print("Target found: "); Serial.println(tag);
      currentState = STATE_FOLLOW_TARGET;
    }
    mfrc522.PICC_HaltA();
  }
  
  // FSM多任务切换
  switch (currentState) {
    case STATE_WAIT:
      Serial.println("Waiting for task...");
      motorL.move(0); motorR.move(0);
      break;
      
    case STATE_FOLLOW_TARGET:
      // 跟随目标（简化为跟随固定距离，实际可扩展人员跟随）
      // 此处模拟目标存在，持续跟随直至接近目标（距离<0.3米）
      float dist = 0.5; // 简化模拟测距（实际需接距离传感器）
      if (dist > 0.3) {
        motorL.move(followSpeed); motorR.move(followSpeed);
      } else {
        currentState = STATE_PICK_EXECUTE;
        Serial.println("Approached target, start picking");
      }
      break;
      
    case STATE_PICK_EXECUTE:
      // 取货执行：精准定位（假设取货需要旋转90度对准货物）
      Serial.println("Executing pick task...");
      // 右转90度（简化控制，实际需闭环角度控制）
      motorL.move(0.3); motorR.move(-0.3);
      delay(800); // 旋转到位
      motorL.move(0); motorR.move(0);
      // 取货完成，触发返回
      currentState = STATE_RETURN;
      turnCount++; // 记录转向
      break;
      
    case STATE_RETURN:
      // 返回等待状态：按转向计数反向回归
      // 反转90度恢复路径
      motorL.move(-0.3); motorR.move(0.3);
      delay(800);
      turnCount--;
      
      if (turnCount == baseTurnCount) {
        currentState = STATE_WAIT;
        Serial.println("Return complete, waiting for next task");
      }
      motorL.move(0); motorR.move(0);
      break;
  }
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(10);
}

// RFID标签ID提取
String getTagUid(MFRC522 &mfrc) {
  byte tagUid[4];
  mfrc.PICC_GetUid(tagUid);
  String uid = "";
  for (byte i = 0; i < 4; i++) {
    uid += String(tagUid[i], HEX);
  }
  return uid.toUpperCase();
}

6、多任务队列调度 + 动态优先级切换
适用场景：仓储多订单并发场景，机器人接收多个任务（取货、放货、补货），按照任务优先级排序，支持动态插入高优先级任务（如紧急补货），当高优先级任务触发时，暂停当前任务并切换，完成后回归原任务，适用于订单密集、紧急任务频发的仓储场景。

核心逻辑：
多任务队列管理：采用环形队列存储任务，每个任务包含类型、优先级、参数，支持任务增删改查；
优先级调度策略：设置任务优先级（紧急补货 > 取货 > 放货），高优先级任务可插入队列头部，优先执行；
任务切换与恢复：任务切换时保存当前任务进度，高优先级任务完成后，自动恢复原任务，通过 FSM 管理任务执行、切换、恢复全流程。

#include <SimpleFOC.h>

// 硬件配置：BLDC底盘 + 任务触发按钮（3类任务）
BLDCMotor motorL(9), motorR(10);
BLDCDriver3PWM drvL(3,5,6), drvR(11,12,13);
Encoder encL(18,19,2048), encR(20,21,2048);

const int btn_pick = 4;   // 取货任务按钮
const int btn_place = 5;  // 放货任务按钮
const int btn_urgent = 6; // 紧急补货按钮（高优先级）

// 任务类型与优先级
enum TaskType { PICK = 0, PLACE = 1, URGENT = 2 };
enum TaskPriority { LOW = 1, MEDIUM = 2, HIGH = 3 };

// 任务结构体
struct Task {
  TaskType type;
  TaskPriority priority;
  String param; // 任务参数（如货物位置）
};

// 任务队列（环形队列，最大容量5）
#define TASK_MAX 5
Task taskQueue[TASK_MAX];
int queueHead = 0, queueTail = 0, queueCount = 0;

// FSM状态
enum FsmState {
  STATE_TASK_EXEC = 0,   // 执行当前任务
  STATE_TASK_SWITCH = 1, // 切换高优先级任务
  STATE_TASK_RESUME = 2  // 恢复原任务
};
FsmState currentState = STATE_TASK_EXEC;

// 当前任务与保存的任务
Task currentTask = {PICK, MEDIUM, "POS_001"};
Task savedTask; // 保存被中断的任务

void setup() {
  Serial.begin(115200);
  // 初始化电机
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  // 初始化按钮引脚
  pinMode(btn_pick, INPUT_PULLUP);
  pinMode(btn_place, INPUT_PULLUP);
  pinMode(btn_urgent, INPUT_PULLUP);
  // 初始化任务队列
  initTaskQueue();
}

void loop() {
  // 检测按钮触发，添加任务
  checkTaskTrigger();
  
  // 任务调度与FSM切换
  if (queueCount > 0) {
    Task nextTask = taskQueue[queueHead];
    // 高优先级任务检查：若新任务优先级高于当前任务
    if (nextTask.priority > currentTask.priority) {
      if (currentState == STATE_TASK_EXEC) {
        savedTask = currentTask; // 保存当前任务
        currentState = STATE_TASK_SWITCH;
        dequeueTask(); // 移除高优先级任务（开始执行）
        currentTask = nextTask;
        Serial.print("Switch to high-priority task: ");
        Serial.println(getTaskName(currentTask.type));
      }
    }
    
    // FSM任务执行
    switch (currentState) {
      case STATE_TASK_EXEC:
        executeTask(currentTask);
        // 任务完成，检查队列是否有下一个任务
        if (isTaskComplete(currentTask)) {
          if (queueCount > 0) {
            dequeueTask();
            currentTask = taskQueue[queueHead];
          } else {
            Serial.println("All tasks completed");
            motorL.move(0); motorR.move(0);
          }
        }
        break;
        
      case STATE_TASK_SWITCH:
        // 执行高优先级任务
        executeTask(currentTask);
        if (isTaskComplete(currentTask)) {
          currentState = STATE_TASK_RESUME;
        }
        break;
        
      case STATE_TASK_RESUME:
        // 恢复被中断的任务
        if (queueCount > 0) {
          queueHead = (queueHead + 1) % TASK_MAX; // 跳过已执行的高优先级任务
          queueCount--;
        }
        currentTask = savedTask;
        currentState = STATE_TASK_EXEC;
        Serial.print("Resume previous task: ");
        Serial.println(getTaskName(currentTask.type));
        break;
    }
  } else {
    motorL.move(0); motorR.move(0);
  }
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

// 初始化任务队列
void initTaskQueue() {
  queueHead = queueTail = queueCount = 0;
}

// 检测任务触发按钮
void checkTaskTrigger() {
  static unsigned long lastTrigger = 0;
  if (millis() - lastTrigger > 500) { // 防抖
    if (digitalRead(btn_pick) == LOW) {
      enqueueTask({PICK, MEDIUM, "POS_002"});
      lastTrigger = millis();
    } else if (digitalRead(btn_place) == LOW) {
      enqueueTask({PLACE, MEDIUM, "POS_003"});
      lastTrigger = millis();
    } else if (digitalRead(btn_urgent) == LOW) {
      enqueueTask({URGENT, HIGH, "POS_004"});
      lastTrigger = millis();
    }
  }
}

// 任务入队
void enqueueTask(Task t) {
  if (queueCount < TASK_MAX) {
    taskQueue[queueTail] = t;
    queueTail = (queueTail + 1) % TASK_MAX;
    queueCount++;
    Serial.print("Task added: "); Serial.println(getTaskName(t.type));
  } else {
    Serial.println("Task queue full!");
  }
}

// 任务出队
void dequeueTask() {
  queueHead = (queueHead + 1) % TASK_MAX;
  queueCount--;
}

// 执行任务（简化逻辑，实际需结合路径规划）
void executeTask(Task t) {
  Serial.print("Executing task: "); Serial.println(getTaskName(t.type));
  // 模拟执行任务：移动到目标位置
  motorL.move(0.3); motorR.move(0.3);
  delay(1000); // 模拟移动过程
}

// 判断任务是否完成（简化：执行1秒后完成）
bool isTaskComplete(Task t) {
  static unsigned long startTime = 0;
  if (startTime == 0) startTime = millis();
  return (millis() - startTime > 1000);
}

// 获取任务名称
String getTaskName(TaskType t) {
  switch (t) {
    case PICK: return "PICK";
    case PLACE: return "PLACE";
    case URGENT: return "URGENT";
    default: return "UNKNOWN";
  }
}

要点解读

1. FSM状态分层：破解动态跟随与多任务的逻辑冲突
   仓储协作场景的核心矛盾是动态跟随的连续性与多任务切换的突发性冲突，FSM状态分层是解决这一矛盾的关键，避免顺序控制导致的卡顿、死循环：
   核心状态分层：将流程拆解为 “基础跟随→任务触发→任务执行→路径恢复” 四层核心状态，明确状态边界：跟随状态专注距离闭环，任务状态专注任务执行，恢复状态专注路径回归，各状态切换依赖明确触发条件（如距离阈值、按钮触发），避免逻辑重叠；
   任务状态的优先级管理：针对不同任务优先级，设计 “可中断任务” 与 “不可中断任务” 两类状态：高优先级任务触发时，中断当前可中断任务，保存任务进度，执行完成后通过恢复状态还原，既保障高优先级任务响应，又避免低优先级任务丢失；
   状态切换的防抖与逻辑校验：在状态切换节点增加防抖处理（如按钮消抖、传感器数据持续阈值判断），同时校验状态切换的合法性（如任务执行状态无法直接切换至跟随状态，需先完成恢复），避免因传感器瞬时干扰导致的状态振荡。
2. 动态跟随的闭环控制：兼顾人员移动灵活性与跟随稳定性
   人机动态跟随是仓储协作的核心需求，需兼顾人员移动的随机性与机器人跟随的稳定性，闭环控制是实现这一目标的核心：
   距离闭环+方向闭环双闭环控制：距离闭环通过PID算法调节电机速度，保持与人员的安全距离；方向闭环通过侧方传感器检测人员方位，采用差速控制调整机器人姿态，确保机器人始终与人员正面对齐，避免仅靠距离闭环导致的 “追尾” 或 “偏离跟随路线”；
   速度自适应调节：根据人员移动速度动态调整机器人跟随速度，例如人员加速时机器人加速，人员减速时机器人减速，同时设置速度限幅（如最大速度0.5m/s、最小速度0.1m/s），既保障跟随的实时性，又避免因速度过快导致碰撞；
   跟随中断的平滑过渡：当跟随被避障或任务切换中断时，保存跟随参数，中断恢复后自动回溯跟随参数，实现跟随的平滑重启，避免重启时的加速过冲或减速滞后。
3. 多任务调度的优先级与队列机制：保障任务高效有序执行
   仓储场景存在多订单并发、紧急任务突发的特点，多任务调度需通过优先级与队列机制，实现任务的高效有序执行：
   优先级分层的任务定义：根据仓储任务紧急程度，明确优先级分层，高优先级任务可中断低优先级任务，确保紧急任务的及时响应，避免低优先级任务占用资源导致紧急任务延误；
   环形队列的高效管理：采用环形队列存储任务，解决线性队列的空间浪费问题，支持任务的快速入队、出队，同时设置队列容量上限，防止队列溢出导致任务丢失，入队时自动按优先级排序，确保队列头部始终为当前最高优先级任务；
   任务中断与恢复的进度保存：对于可中断的低优先级任务，在切换时保存任务进度（如路径转向计数、目标位置参数），高优先级任务完成后，通过进度参数恢复原任务，避免任务重复执行或路径丢失，保障任务的连贯性。
4. BLDC电机的适配控制：保障跟随与任务执行的精准性
   BLDC电机是机器人运动的核心执行单元，需针对仓储跟随、任务切换的场景需求，进行控制参数与模式的适配，保障动作的精准性与稳定性：
   控制模式的场景适配：跟随场景采用速度闭环控制，保障速度跟随的实时性；任务执行场景采用位置闭环控制，保障取货、放货时的姿态对准精度，不同场景自动切换控制模式，避免控制模式错配导致的动作偏差；
   差速控制的路径修正：差速底盘通过左右轮速度差实现转向，在跟随过程中，根据人员方位偏差动态调整差速，修正机器人姿态，避免因单一速度控制导致的转向滞后；在避障或任务切换时，差速控制快速响应转向需求，实现路径的精准调整；
   参数限幅与安全保障：对电机速度、加速度进行限幅，避免因突发指令导致的速度突变，保障机械结构安全；同时设置堵转保护，当电机因障碍物卡住超过设定时间，自动停机并报警，防止电机过热损坏。
5. 仓储场景的安全与鲁棒性设计：应对动态环境不确定性
   仓储环境存在人员走动、货物堆放、通道狭窄等不确定性，安全与鲁棒性是协作机器人的底线，需从硬件、软件多维度保障：
   多传感器融合的障碍物检测：融合前方、侧方传感器，实现360°障碍物检测，避免单一传感器盲区导致漏检；采用传感器数据互补与校验，提升障碍物检测的准确性，防止因单个传感器失效导致的碰撞风险；
   安全距离的双重保障：设置软安全距离（PID调节的目标距离）与硬安全距离（紧急停机阈值），当障碍物距离小于软安全距离时，自动减速调整；小于硬安全距离时，立即停机并进入避障状态，双重保障避免碰撞；
   故障自恢复机制：针对传感器故障、电机堵转、通信中断等异常，设计自恢复机制，如传感器故障时自动切换至预设安全模式（低速运行），电机堵转时触发报警并等待人工干预，确保机器人在异常情况下可快速进入安全状态，避免故障扩大化；
   状态反馈与可视化：通过串口输出机器人状态、任务进度、传感器数据，便于运维人员实时监控，同时预留LED指示灯接口，实现状态的本地可视化，方便现场人员快速判断机器人运行状态，提升故障排查效率。

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