Arduino BLDC之智能仓储拣货机器人（FSM驱动+任务队列+状态指示）是一套以有限状态机为决策核心、任务队列为执行载体、状态指示为运维窗口的三层解耦机器人控制系统，其核心优势在于将复杂的拣货流程拆解为确定性的状态转换，主要适用于中小型仓储、教育科研及轻量级自动化场景，但落地时需重点解决算力瓶颈、状态死锁与电源隔离等工程问题。

1、系统架构与技术原理
该系统采用"决策-调度-执行"三层解耦架构，将行为逻辑、任务管理与电机驱动彻底分离：
FSM决策层：有限状态机（FSM）是系统的"大脑"，由状态（State）、事件（Event）、转换（Transition）三要素构成。典型状态包括：IDLE（空闲等待）、NAVIGATING（导航中）、PICKING（拣货中）、RETURNING（返程中）、CHARGING（充电中）、ERROR（故障）。每个状态对应一组确定的行为动作，事件触发状态之间的确定性转换，例如"收到新任务"事件将系统从IDLE切换到NAVIGATING。
任务队列调度层：系统内部维护一个FIFO（先进先出）或优先级队列，存储待执行的拣货任务（包含目标货架坐标、SKU编号、数量等）。上位机（WMS/WCS）通过无线通信下发任务，MCU将其入队；FSM每完成一个任务周期后自动出队并执行下一个。
BLDC执行驱动层：基于Arduino生态（如SimpleFOC库），通过FOC（磁场定向控制）驱动无刷电机，实现位置/速度/力矩三环闭环控制，确保机器人在导航、停靠、取放货等环节的精准运动。

2、主要特点
FSM驱动的确定性行为控制
逻辑清晰、可预测：FSM将复杂的拣货流程拆解为有限的离散状态，系统在任何时刻都处于且仅处于一个确定状态，避免了传统"意大利面条式"代码中条件分支交叉混乱的问题。
Moore型与Mealy型混合设计：状态内部的动作（如导航时持续运行PID速度环）采用Moore型——输出仅依赖当前状态；状态转换时的瞬时动作（如到达目标点时触发机械臂抓取）采用Mealy型——输出依赖当前事件，使逻辑表达最为清晰。
异常处理天然内嵌：任何状态下均可响应EMERGENCY_STOP、OBSTACLE_DETECTED、LOW_BATTERY等异常事件，统一跳转至ERROR或SAFE_STOP状态，确保系统在任何异常下都有明确的退出路径，不会陷入未定义行为。
任务队列的弹性调度
异步任务接收：上位机可随时向队列中追加任务，无需等待当前任务完成。MCU通过中断或轮询方式接收无线指令并入队，实现任务下发与执行的完全解耦。
动态优先级插入：支持紧急任务（如补货优先级高于常规拣货）插入队列头部，或在队列中按任务紧急度、货架距离等条件进行排序，实现轻量级的任务优先级仲裁。
断点续执行：当任务因低电量中断时，当前任务信息保留在队列中，充电完成后自动恢复执行，无需上位机重新下发。
多维度状态指示
本地可视化：通过LED阵列或OLED屏幕实时显示当前状态（如绿灯慢闪=空闲、蓝灯快闪=导航中、黄灯常亮=拣货中、红灯闪烁=故障），使现场运维人员无需连接电脑即可判断机器人运行状况。
远程状态上报：通过Wi-Fi/MQTT协议周期性向上位机上报当前状态、队列深度、电池SOC、电机温度等信息，支持WMS系统实时监控多台机器人的运行态势。
故障码标准化：不同故障类型对应不同的LED闪烁频率或错误码（如0x01=编码器故障、0x02=电机过流、0x03=通信超时），便于快速定位问题。
BLDC高效精准执行
高效率长续航：BLDC电机效率>85%，配合FOC正弦波驱动消除转矩脉动，机器人启停极其平滑，有效保护所承载的货物。
多模式切换：导航阶段采用速度闭环维持巡航效率，接近目标点时切换至位置闭环实现毫米级精准停靠，拣货阶段切换至力矩闭环保持机械臂柔顺抓取。

3、典型应用场景
中小型电商仓库的"货到人"拣货
在SKU种类多、订单碎片化的电商仓库中，机器人根据WMS下发的拣货任务队列，依次导航至各货架位，通过二维码/RFID定位精准停靠，配合升降机构或机械臂完成取货，再运送至拣货工作站。FSM确保每个环节严格按序执行，任务队列保证多订单的高效串行处理。
工厂线边物流配送
在生产车间内，机器人根据工位的物料呼叫请求（入队），按优先级依次从仓库取料并配送至指定工位。FSM管理"等待呼叫→取料→配送→返回"的完整循环，状态指示让车间工人直观了解机器人的当前位置和预计到达时间。
图书馆/档案馆的自动归架
机器人接收归还书籍的扫码信息后入队，依次导航至对应书架位置完成上架。任务队列支持批量处理，FSM确保机器人在密集书架通道中的安全行驶和精准定位。
教育科研与算法验证平台
作为机器人学、嵌入式系统、状态机理论的教学实验平台，学生可以直观观察FSM状态转换过程，修改任务调度策略，验证新的路径规划或避障算法，是低成本、高可视化的理想验证环境。
医院药品/样本配送
在楼宇内自动运送药品、检验样本或医疗器械，FSM管理电梯呼叫、楼层切换、走廊导航等复杂流程，任务队列支持多科室请求的优先级排序。

4、注意事项与关键技术挑战
MCU算力与实时性瓶颈
Arduino Uno/Nano等8位MCU主频仅16MHz，难以同时处理FSM推理、任务队列管理、PID运算和无线通信，容易导致控制周期不稳定。
对策：必须选用带硬件FPU的32位MCU（如ESP32-S3双核、STM32F4），或采用"上位机（树莓派/Jetson）负责FSM与任务调度 + Arduino作为底层BLDC执行器"的分层架构，通过UART/SPI高速通信解耦。
FSM状态死锁与非法转换
若状态转换表设计不严谨，可能出现"两个状态互相等待"的死锁，或因传感器噪声触发非法转换导致系统崩溃。
对策：编码前必须绘制完整的状态转换图（STD），穷举所有可能的状态×事件组合；为每个状态设置超时保护（如NAVIGATING超过预定时间未到达则跳转ERROR）；启用硬件看门狗定时器防止程序跑飞。
任务队列的线程安全
无线接收中断与主循环同时访问任务队列时，若不加保护会导致数据竞争和队列损坏。
对策：采用SPSC（单生产者单消费者）环形缓冲区模型——中断服务程序（ISR）只负责写入，主循环只负责读取，通过head/tail指针天然避免竞争；若使用FreeRTOS，则直接使用线程安全的xQueueSend/xQueueReceive API。
电源隔离与电磁兼容（EMC）
BLDC电机启停和堵转时产生的大电流尖峰会通过共地线路干扰MCU，导致FSM状态错乱或通信中断。
对策：电机驱动电源与MCU逻辑电源必须物理隔离（独立DC-DC模块）；电机相线套屏蔽网；母线并联≥470μF电解电容抑制电压跌落；急停信号采用硬件中断直接切断电机使能，绕过软件调度层。
通信可靠性与离线容错
仓库中金属货架会造成Wi-Fi信号遮挡和多径反射，通信中断时机器人可能丢失任务或停在通道中央。
对策：MCU端必须本地缓存任务队列，网络恢复后自动续传上报状态；通信协议加入CRC校验和心跳检测；连续N次心跳丢失后自动进入安全停车状态。
编码器精度与里程计漂移
仅靠编码器积分的里程计会随时间累积误差，导致机器人在长距离导航后偏离目标货架。
对策：在关键节点（如货架位、充电座）部署二维码/RFID地标进行绝对位置校正；结合IMU进行航向角补偿；定期执行"归零校准"消除累积误差。
安全合规性
拣货机器人在人机混行环境中运行，必须满足工业车辆安全标准（如ISO 3691-4）。
对策：配备激光雷达/超声波阵列实现动态避障；设置空载/满载不同速度档位；急停按钮符合ISO 13850标准；FSM中任何状态下均可响应急停事件并立即切断电机动力。

1、基础FSM顺序拣货（固定航点）
场景：标准化仓库中，机器人按固定路径依次前往多个货架点位，执行“到达→取货→前往下一站”的循环。

核心逻辑：将整个拣货任务分解为离散状态（IDLE→MOVING→LOADING→UNLOADING→RETURNING），用FSM管理状态间的跳转。每个状态负责一段行为逻辑：前进至目标点、执行取货动作、前往下一站点等。到达一个航点后执行动作并切换至下一个任务索引。

#include <SimpleFOC.h>

// ===== BLDC差速电机 =====
BLDCMotor motorL(7), motorR(8);

// ===== FSM状态枚举 =====
enum RobotState {
    STATE_IDLE,
    STATE_MOVING_TO_SHELF,    // 前往货架
    STATE_LOADING,            // 取货
    STATE_MOVING_TO_DROP,     // 前往卸货点
    STATE_UNLOADING,          // 卸货
    STATE_RETURNING,          // 返回充电/起点
    STATE_EMERGENCY_STOP      // 紧急状态
};
RobotState currentState = STATE_IDLE;

// ===== 拣货任务队列（货架坐标，简化用脉冲数）=====
struct PickTask {
    int targetPulse;          // 目标位置（编码器脉冲）
    int shelfID;              // 货架编号
    int actionDuration;       // 取货/卸货停留时间(ms)
};
PickTask pickQueue[] = {
    {1000, 1, 2000},   // 货架1：前进至1000脉冲，停留2秒
    {2500, 2, 1500},   // 货架2
    {4000, 3, 3000}    // 货架3
};
int numTasks = sizeof(pickQueue) / sizeof(pickQueue[0]);
int currentTaskIndex = 0;

// ===== 状态指示（LED+蜂鸣器）=====
#define LED_RED 3
#define LED_GREEN 4
#define LED_BLUE 5
#define BUZZER 6

// ===== 控制变量 =====
const int SPEED_FORWARD = 130;   // PWM值
bool motorRunning = false;

void setup() {
    Serial.begin(115200);
    pinMode(LED_RED, OUTPUT);
    pinMode(LED_GREEN, OUTPUT);
    pinMode(LED_BLUE, OUTPUT);
    pinMode(BUZZER, OUTPUT);
    // BLDC电机初始化...
    currentState = STATE_IDLE;
    Serial.println("拣货机器人启动，等待指令");
}

void loop() {
    // 1. 获取编码器脉冲
    long currentPulse = getEncoderPulse();

    // 2. FSM主循环
    switch (currentState) {
        case STATE_IDLE: {
            motorL.move(0); motorR.move(0);
            setIndicator(0, 255, 0);  // 绿灯：待命
            // 收到启动指令后进入拣货状态
            if (Serial.read() == 'S') {
                currentState = STATE_MOVING_TO_SHELF;
                Serial.println("→ 开始拣货任务");
            }
            break;
        }

        case STATE_MOVING_TO_SHELF: {
            setIndicator(255, 255, 0);  // 黄灯：行驶中
            int targetPulse = pickQueue[currentTaskIndex].targetPulse;
            int error = targetPulse - currentPulse;
            
            if (abs(error) < 10) {  // 到达目标
                motorL.move(0); motorR.move(0);
                motorRunning = false;
                currentState = STATE_LOADING;
                Serial.print("→ 到达货架"); Serial.println(pickQueue[currentTaskIndex].shelfID);
            } else {
                int speed = constrain(abs(error) * 0.5, 30, SPEED_FORWARD);
                motorL.move(error > 0 ? speed : -speed);
                motorR.move(error > 0 ? speed : -speed);
                motorRunning = true;
            }
            break;
        }

        case STATE_LOADING: {
            setIndicator(0, 0, 255);  // 蓝灯：执行取货
            static unsigned long loadStart = 0;
            if (loadStart == 0) {
                loadStart = millis();
                digitalWrite(BUZZER, HIGH);
                delay(100);
                digitalWrite(BUZZER, LOW);
                Serial.print("→ 取货中 (货架"); Serial.print(pickQueue[currentTaskIndex].shelfID);
                Serial.println(")");
            }
            if (millis() - loadStart > pickQueue[currentTaskIndex].actionDuration) {
                loadStart = 0;
                currentTaskIndex++;
                if (currentTaskIndex >= numTasks) {
                    currentState = STATE_MOVING_TO_DROP;
                } else {
                    currentState = STATE_MOVING_TO_SHELF;
                }
            }
            break;
        }

        case STATE_MOVING_TO_DROP: {
            setIndicator(255, 255, 0);  // 黄灯：行驶中
            // 前往卸货点（固定位置）
            int dropPulse = 5000;  // 卸货点编码器脉冲
            int error = dropPulse - currentPulse;
            if (abs(error) < 10) {
                motorL.move(0); motorR.move(0);
                currentState = STATE_UNLOADING;
                Serial.println("→ 到达卸货点");
            } else {
                int speed = constrain(abs(error) * 0.5, 30, SPEED_FORWARD);
                motorL.move(error > 0 ? speed : -speed);
                motorR.move(error > 0 ? speed : -speed);
            }
            break;
        }

        case STATE_UNLOADING: {
            setIndicator(0, 255, 255);  // 青色：卸货中
            static unsigned long unloadStart = 0;
            if (unloadStart == 0) {
                unloadStart = millis();
                digitalWrite(BUZZER, HIGH);
                delay(200);
                digitalWrite(BUZZER, LOW);
                Serial.println("→ 卸货中");
            }
            if (millis() - unloadStart > 2000) {
                unloadStart = 0;
                currentState = STATE_RETURNING;
                Serial.println("→ 任务完成，返回起点");
            }
            break;
        }

        case STATE_RETURNING: {
            setIndicator(255, 0, 0);  // 红灯：返回中
            if (abs(currentPulse) < 10) {
                motorL.move(0); motorR.move(0);
                currentState = STATE_IDLE;
                setIndicator(0, 255, 0);
                Serial.println("→ 已回到起点，等待新任务");
            } else {
                motorL.move(-SPEED_FORWARD * 0.6);
                motorR.move(-SPEED_FORWARD * 0.6);
            }
            break;
        }

        case STATE_EMERGENCY_STOP: {
            motorL.move(0); motorR.move(0);
            setIndicator(255, 0, 0);  // 红灯快闪
            digitalWrite(BUZZER, HIGH);
            Serial.println("⚠ 紧急停止！");
            break;
        }
    }

    // 3. 状态指示器（当前状态输出到串口/屏幕）
    printStateToSerial();
    delay(50);
}

void setIndicator(int r, int g, int b) {
    analogWrite(LED_RED, r);
    analogWrite(LED_GREEN, g);
    analogWrite(LED_BLUE, b);
}

void printStateToSerial() {
    static RobotState lastState = STATE_IDLE;
    if (currentState != lastState) {
        Serial.print("当前状态: ");
        switch(currentState) {
            case STATE_IDLE: Serial.println("待命"); break;
            case STATE_MOVING_TO_SHELF: Serial.println("前往货架"); break;
            case STATE_LOADING: Serial.println("取货中"); break;
            case STATE_MOVING_TO_DROP: Serial.println("前往卸货点"); break;
            case STATE_UNLOADING: Serial.println("卸货中"); break;
            case STATE_RETURNING: Serial.println("返回起点"); break;
            case STATE_EMERGENCY_STOP: Serial.println("紧急停止"); break;
        }
        lastState = currentState;
    }
}

2、双机协同 + 任务队列（动态分配）
场景：两台拣货机器人在同一仓储区域作业，通过无线通信（UWB/ESP-NOW）共享任务负载，实现动态任务分配与负载均衡。

核心逻辑：借鉴“双机器人智能补货”架构，每台机器人维护自己的任务队列，通过无线交换任务数量和状态。负载均衡采用模糊协商——电量高、距任务点近的机器人承接更多任务；当某机任务受阻（如堵转），可通过任务窃取（Work Stealing）广播释放任务。

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

// ===== BLDC差速电机 =====
BLDCMotor motorL(7), motorR(8);

// ===== 拣货任务结构 =====
struct PickTask {
    int id;
    int shelfX, shelfY;    // 货架坐标（栅格）
    bool active;
    unsigned long assignedTime;
    int priority;          // 1=普通, 2=紧急（低库存）
};

// ===== 任务队列（环形缓冲区）=====
#define MAX_TASKS 10
PickTask myQueue[MAX_TASKS];
int taskCount = 0;

// ===== 邻机状态（无线接收）=====
struct PeerState {
    int id;
    float battery;         // 0~1
    int taskCount;
    float x, y;
};
PeerState peer;

// ===== 机器人自身状态 =====
float selfBattery = 1.0;
float selfX = 0, selfY = 0;
bool isStuck = false;

// ===== 任务分配函数 =====
float evalPriority(float bat, float distToTask) {
    float p = 0.5;
    p += bat * 0.3;
    p += (1.0 - constrain(distToTask / 20.0, 0, 1)) * 0.3;
    return constrain(p, 0.2, 0.9);
}

void loop() {
    // 1. 更新自身状态
    updateBattery();
    updatePosition();

    // 2. 接收邻机状态（非阻塞）
    if (esp_now_receive_peer(&peer)) {
        // 已更新邻机数据
    }

    // 3. 【核心】负载均衡：比较自身与邻机的任务队列长度
    int myTasks = taskCount;
    int peerTasks = peer.taskCount;

    if (myTasks > peerTasks + 2) {
        // 自身任务过多，让渡非紧急任务给邻机
        for (int i = 0; i < myTasks; i++) {
            if (myQueue[i].priority == 1 && myQueue[i].active) {
                sendTaskToPeer(myQueue[i]);
                myQueue[i].active = false;
                taskCount--;
                Serial.printf("让渡任务 #%d 给机器人 %d\n", myQueue[i].id, peer.id);
                break;
            }
        }
    }

    // 4. 若邻机任务过多且自身空闲，窃取任务
    if (myTasks == 0 && peerTasks > 3) {
        if (requestTaskFromPeer()) {
            // 成功窃取，更新队列
        }
    }

    // 5. 执行当前任务
    if (taskCount > 0) {
        executeNextTask();
    } else {
        goToChargingStation();  // 无任务时回充
    }

    delay(50);
}

3、完整拣货闭环 + MQTT任务下发 + 状态上报
场景：与WMS（仓库管理系统）对接，通过MQTT接收任务，执行后上报完成状态。货架库存低于阈值自动生成补货任务。

核心逻辑：采用FSM管理拣货生命周期——从接收任务、导航到货架、取货、卸货到上报完成，每个状态切换时通过MQTT同步状态。任务队列由WMS通过MQTT动态填充，支持紧急任务插队。

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

// ===== BLDC差速电机 =====
BLDCMotor motorL(7), motorR(8);

// ===== MQTT配置 =====
WiFiClient espClient;
PubSubClient mqtt(espClient);
#define MQTT_TOPIC_TASK "warehouse/robot/task"
#define MQTT_TOPIC_STATUS "warehouse/robot/status"

// ===== FSM状态 =====
enum State { IDLE, MOVING, PICKING, DROPPING, COMPLETE, ERROR };
State currentState = IDLE;

// ===== 当前任务 =====
struct Task {
    int id;
    int shelfX, shelfY;
    int dropX, dropY;
    bool isEmergency;
};
Task currentTask;
bool hasTask = false;

// ===== MQTT回调：接收WMS下发的任务 =====
void mqttCallback(char* topic, byte* payload, unsigned int length) {
    if (strcmp(topic, MQTT_TOPIC_TASK) == 0) {
        String msg = String((char*)payload).substring(0, length);
        // 解析JSON格式任务: {"id":101,"shelfX":3,"shelfY":5,"dropX":0,"dropY":0,"emergency":false}
        parseTask(msg, &currentTask);
        hasTask = true;
        currentState = MOVING;
        mqtt.publish(MQTT_TOPIC_STATUS, "状态:前往货架");
        Serial.printf("收到任务 #%d\n", currentTask.id);
    }
}

void loop() {
    // 1. MQTT保活
    mqtt.loop();

    // 2. FSM任务执行
    switch (currentState) {
        case IDLE:
            // 空闲，等待任务
            break;

        case MOVING: {
            // 导航到货架坐标
            if (navigateTo(currentTask.shelfX, currentTask.shelfY)) {
                currentState = PICKING;
                mqtt.publish(MQTT_TOPIC_STATUS, "状态:取货中");
            }
            break;
        }

        case PICKING: {
            // 执行取货动作（机械臂/夹爪）
            if (performPickup()) {
                currentState = DROPPING;
                mqtt.publish(MQTT_TOPIC_STATUS, "状态:前往卸货点");
            }
            break;
        }

        case DROPPING: {
            if (navigateTo(currentTask.dropX, currentTask.dropY)) {
                if (performDropoff()) {
                    currentState = COMPLETE;
                    // 上报任务完成
                    mqtt.publish(MQTT_TOPIC_STATUS, "状态:任务完成");
                    Serial.printf("任务 #%d 完成\n", currentTask.id);
                    hasTask = false;
                    currentState = IDLE;
                }
            }
            break;
        }

        case ERROR:
            // 故障处理，上报错误
            mqtt.publish(MQTT_TOPIC_STATUS, "状态:故障");
            break;
    }

    delay(50);
}

要点解读
FSM是“行为确定性”的保障，但需警惕状态爆炸：FSM将复杂拣货流程分解为离散的、有限的状态，每个状态有明确的入口和出口逻辑。当状态数量超过10个时，转移条件会变得极其复杂，建议采用分层状态机（Hierarchical FSM）——顶层管理大流程（巡航、暂停、故障），每个顶层状态内部嵌套子状态机管理细节。

任务队列结合负载均衡是实现“多机协同”的核心：智能仓储场景中多机协同依赖动态任务分配。通过无线通信交换任务数量和电量状态，采用模糊协商——距离任务点近、电量高的机器人优先承接任务。任务窃取（Work Stealing）机制可防止单点死锁。

状态指示是工程调试的“生命线”：FSM的每个状态切换本质上是离散的。绑定LED指示灯（红/黄/蓝/青/绿对应不同状态）能让调试者不依赖上位机即可快速定位机器人所处的逻辑阶段。结合串口打印状态名称，对排查“死锁”和“意外状态跳转”至关重要。

BLDC软启动与过流保护是频繁启停场景的“安全红线”：拣货机器人频繁启停，必须避免直接给最大PWM值启动，否则会导致抖动、打滑，影响定位精度。最佳实践是在状态机内部加入软启动逻辑（Ramp Up），PWM值从0逐渐增加到目标值。BLDC启动电流可达额定值的3~5倍，必须设置软件过流保护。

控制实时性保障：严禁使用delay()阻塞：控制回路必须使用硬件定时器中断或millis()非阻塞定时，确保采样周期的精确性。电机控制频率建议≥50Hz。在ESP32双核架构中，可Core0负责FSM决策和通信，Core1负责电机控制和传感器采样，实现任务隔离。

4、平库货架拣货机器人——FSM全状态覆盖+循环任务队列+声光指示
适用场景：中小型平库的多货架拣货（如电商小件仓），要求机器人自动完成待机→导航→拣货→返回→待机全流程，支持任务批量录入、队列化处理，同时通过声光实时反馈状态，便于仓库人员监控。

核心设计：
FSM状态机：定义5个核心状态，覆盖拣货全流程，状态切换基于传感器触发和任务完成条件；
循环任务队列：用数组模拟循环队列存储拣货任务，支持队列增删、出队处理；
状态指示：LED灯（红/黄/绿）指示待机、导航、拣货、故障状态，蜂鸣器提示任务完成和异常。

/* 平库拣货机器人：FSM驱动+循环任务队列+声光状态指示
   核心逻辑：全流程状态覆盖（待机/导航/拣货/返回/故障）+ 循环任务队列 + 声光反馈
   硬件：Arduino Mega + 2路BLDC伺服（导航/拣货）+ 激光雷达（避障）+ 货架RFID读写器 + RGB LED + 蜂鸣器
   状态参数：待机超时5s，导航速度10，拣货等待2s
*/

// --- FSM状态定义 ---
typedef enum {
  STATE_IDLE = 0,     // 待机（等待任务/充电）
  STATE_NAVIGATE,     // 导航至货架
  STATE_PICKING,      // 执行拣货动作
  STATE_RETURNING,    // 返回分拣点
  STATE_FAULT         // 故障停机
} FSMState;

FSMState currentState = STATE_IDLE;
unsigned long stateStartTime = 0;
const unsigned long IDLE_TIMEOUT = 5000;  // 待机超时时间（ms）

// --- 任务队列定义 ---
struct PickTask {
  uint8_t shelfId;     // 货架ID
  uint8_t itemId;      // 货物ID
  uint8_t qty;         // 拣货数量
};
#define TASK_QUEUE_SIZE 10
PickTask taskQueue[TASK_QUEUE_SIZE];
uint8_t queueHead = 0, queueTail = 0, taskCount = 0;

// --- 硬件引脚定义 ---
#define NAV_MOTOR_PWM 9   // 导航电机（水平移动）
#define PICKING_MOTOR_PWM 10 // 拣货电机（垂直抓取）
#define LASER_TRIG 11     // 激光雷达触发
#define LASER_ECHO 12     // 激光雷达回波
#define RFID_CLK 13
#define RFID_DATA 14
#define RGB_LED_R 15
#define RGB_LED_G 16
#define RGB_LED_B 17
#define BUZZER 18

// --- 任务队列操作函数 ---
bool enqueueTask(PickTask task) {
  if ((queueTail + 1) % TASK_QUEUE_SIZE == queueHead) {
    Serial.println("任务队列已满！");
    return false; // 队列满，拒绝添加
  }
  taskQueue[queueTail] = task;
  queueTail = (queueTail + 1) % TASK_QUEUE_SIZE;
  taskCount++;
  return true;
}

PickTask dequeueTask() {
  if (taskCount == 0) {
    Serial.println("任务队列为空！");
    return {0, 0, 0}; // 空任务
  }
  PickTask task = taskQueue[queueHead];
  queueHead = (queueHead + 1) % TASK_QUEUE_SIZE;
  taskCount--;
  return task;
}

bool isEmptyQueue() { return taskCount == 0; }

// --- FSM状态处理函数 ---
void handleIdleState() {
  if (isEmptyQueue()) {
    // 无任务，保持待机，触发超时进入充电/待机休眠（简化：循环待机）
    setStatusLED(0, 1, 0); // 绿灯常亮
    if (millis() - stateStartTime > IDLE_TIMEOUT) {
      Serial.println("待机超时，等待新任务");
      stateStartTime = millis();
    }
  } else {
    // 有任务，切换至导航状态
    currentState = STATE_NAVIGATE;
    stateStartTime = millis();
    Serial.println("切换至导航状态");
  }
}

void handleNavigateState() {
  if (isEmptyQueue()) {
    currentState = STATE_IDLE;
    return;
  }
  PickTask task = taskQueue[queueHead]; // 预览任务（不弹出）
  Serial.printf("导航至货架%d，距离150cm\n", task.shelfId);
  // 模拟导航移动（BLDC驱动轮移动，简化为等待导航完成）
  moveToShelf(task.shelfId);
  // 导航完成条件：激光雷达检测到货架（简化：延时后判断到达）
  if (millis() - stateStartTime > 3000) {
    currentState = STATE_PICKING;
    stateStartTime = millis();
    Serial.println("导航完成，切换至拣货状态");
  }
}

void handlePickingState() {
  if (isEmptyQueue()) {
    currentState = STATE_IDLE;
    return;
  }
  PickTask task = dequeueTask(); // 出队任务
  Serial.printf("拣取货物%d，数量%d\n", task.itemId, task.qty);
  // 模拟拣货动作：垂直电机下降→抓取→上升（简化为延时）
  pickingAction(task.qty);
  // 拣货完成条件：RFID确认货物拾取（简化：延时后判断完成）
  if (millis() - stateStartTime > 2000) {
    currentState = STATE_RETURNING;
    stateStartTime = millis();
    Serial.println("拣货完成，切换至返回状态");
  }
}

void handleReturningState() {
  Serial.println("返回分拣点");
  // 模拟返回动作（BLDC驱动轮返回）
  moveToBase();
  // 返回完成条件：到达分拣点（简化：延时后判断完成）
  if (millis() - stateStartTime > 3000) {
    currentState = STATE_IDLE;
    stateStartTime = millis();
    Serial.println("返回完成，切换至待机状态");
    // 任务完成提示
    beep(200, 100); // 蜂鸣200ms，间隔100ms
  }
}

void handleFaultState() {
  setStatusLED(1, 0, 0); // 红灯常亮
  beep(500, 200); // 长鸣提示故障
  // 停止所有电机
  analogWrite(NAV_MOTOR_PWM, 0);
  analogWrite(PICKING_MOTOR_PWM, 0);
  // 等待复位指令（简化：延时后切换回待机）
  if (millis() - stateStartTime > 3000) {
    currentState = STATE_IDLE;
    stateStartTime = millis();
    Serial.println("故障复位，返回待机状态");
  }
}

// --- 状态指示函数 ---
void setStatusLED(int r, int g, int b) {
  digitalWrite(RGB_LED_R, r);
  digitalWrite(RGB_LED_G, g);
  digitalWrite(RGB_LED_B, b);
}

void beep(int duration, int interval) {
  digitalWrite(BUZZER, HIGH);
  delay(duration);
  digitalWrite(BUZZER, LOW);
  delay(interval);
}

// --- 模拟电机动作函数 ---
void moveToShelf(uint8_t shelfId) {
  analogWrite(NAV_MOTOR_PWM, 200); // 导航电机运转
}

void pickingAction(uint8_t qty) {
  analogWrite(PICKING_MOTOR_PWM, 150); // 拣货电机动作
}

void moveToBase() {
  analogWrite(NAV_MOTOR_PWM, -200); // 返回电机反转
}

// --- 故障检测函数 ---
bool checkFault() {
  // 模拟故障：激光雷达检测障碍物过近
  long distance = pulseIn(LASER_ECHO, HIGH) * 0.0343 / 2;
  if (distance < 10) { // 障碍物距离<10cm
    Serial.println("检测到障碍物，触发故障");
    return true;
  }
  return false;
}

void setup() {
  Serial.begin(115200);
  // 初始化引脚
  pinMode(LASER_TRIG, OUTPUT);
  pinMode(LASER_ECHO, INPUT);
  pinMode(RGB_LED_R, OUTPUT);
  pinMode(RGB_LED_G, OUTPUT);
  pinMode(RGB_LED_B, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  pinMode(NAV_MOTOR_PWM, OUTPUT);
  pinMode(PICKING_MOTOR_PWM, OUTPUT);
  // 初始化状态
  stateStartTime = millis();
  // 添加初始任务
  enqueueTask({1, 101, 3});
  enqueueTask({2, 102, 5});
  Serial.println("初始化完成，添加2个拣货任务");
}

void loop() {
  // 故障检测（优先处理）
  if (checkFault()) {
    currentState = STATE_FAULT;
    stateStartTime = millis();
  }
  // FSM状态切换
  switch (currentState) {
    case STATE_IDLE:
      handleIdleState();
      break;
    case STATE_NAVIGATE:
      handleNavigateState();
      break;
    case STATE_PICKING:
      handlePickingState();
      break;
    case STATE_RETURNING:
      handleReturningState();
      break;
    case STATE_FAULT:
      handleFaultState();
      break;
  }
  delay(50); // 控制循环频率
}

5、立体库高位拣货机器人——FSM状态分层+任务优先级队列+可视化指示
适用场景：立体库高位货架拣货（如自动化立体仓库），要求机器人具备提升机构控制、高度定位、任务优先级处理能力，同时通过数码管和指示灯显示当前状态、任务进度，便于中控系统监控。

核心设计：
FSM状态分层：在基础状态机中加入提升控制子状态，覆盖平层移动、垂直提升、高度定位全流程；
优先级任务队列：任务按优先级排序（高优先级优先处理），支持紧急任务插队；
可视化状态指示：数码管显示当前状态、货架层数，LED指示运行/待机/提升中状态。

/* 立体库高位拣货机器人：FSM分层+优先级任务队列+数码管指示
   核心逻辑：FSM加入提升子状态 + 优先级任务队列 + 数码管状态显示
   硬件：ESP32 + 3路BLDC（导航/提升/抓取）+ 高度编码器 + 数码管（7段） + 高优先级任务按钮
   状态参数：提升速度5，提升高度定位精度±5cm
*/

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <TM1637Display.h>

// --- FSM状态定义（含提升子状态） ---
typedef enum {
  STATE_IDLE = 0,
  STATE_NAVIGATE,
  STATE_ELEVATING,  // 提升机构动作
  STATE_PICKING,
  STATE_RETURNING,
  STATE_FAULT
} FSMState;

// 提升子状态
typedef enum {
  SUB_STATE_HORIZONTAL = 0, // 水平移动
  SUB_STATE_VERTICAL,       // 垂直提升
  SUB_STATE_ALIGN           // 高度对准
} SubState;

FSMState currentState = STATE_IDLE;
SubState subState = SUB_STATE_HORIZONTAL;
unsigned long stateStartTime = 0;

// --- 优先级任务队列 ---
struct PickTask {
  uint8_t shelfId;
  uint8_t itemId;
  uint8_t qty;
  uint8_t priority; // 0-3，3为最高优先级（紧急任务）
};
// 任务队列按优先级排序（高优先级在前）
PickTask taskQueue[10];
uint8_t taskCount = 0;

// --- 数码管初始化 ---
#define CLK 4
#define DIO 5
TM1637Display display(CLK, DIO);

// --- 硬件引脚定义 ---
#define NAV_MOTOR 18   // 水平导航电机
#define ELEVATOR_MOTOR 19 // 提升电机
#define GRAB_MOTOR 21  // 抓取电机
#define PRIORITY_BUTTON 22 // 紧急任务按钮
#define HEIGHT_ENCODER A0 // 高度编码器（模拟输入）

// --- 任务队列操作（优先级排序） ---
void sortTaskQueue() {
  for (int i = 0; i < taskCount - 1; i++) {
    for (int j = 0; j < taskCount - i - 1; j++) {
      if (taskQueue[j].priority < taskQueue[j+1].priority) {
        PickTask temp = taskQueue[j];
        taskQueue[j] = taskQueue[j+1];
        taskQueue[j+1] = temp;
      }
    }
  }
}

bool enqueueTask(PickTask task) {
  if (taskCount >= 10) {
    Serial.println("任务队列已满！");
    return false;
  }
  taskQueue[taskCount++] = task;
  sortTaskQueue(); // 插入后重新排序
  return true;
}

PickTask dequeueTask() {
  if (taskCount == 0) return {0,0,0,0};
  PickTask task = taskQueue[0];
  for (int i = 0; i < taskCount - 1; i++) {
    taskQueue[i] = taskQueue[i+1];
  }
  taskCount--;
  return task;
}

bool isEmptyQueue() { return taskCount == 0; }

// --- FSM状态处理（含提升子状态） ---
void handleIdleState() {
  setStatusIndicator(0); // 待机：绿灯
  displayState("IDLE");
  if (!isEmptyQueue() || digitalRead(PRIORITY_BUTTON) == HIGH) {
    if (digitalRead(PRIORITY_BUTTON) == HIGH) {
      // 紧急任务插队：添加最高优先级任务
      enqueueTask({5, 205, 1, 3});
      Serial.println("检测到紧急任务，已插队");
    }
    currentState = STATE_NAVIGATE;
    subState = SUB_STATE_HORIZONTAL;
    stateStartTime = millis();
    displayState("NAV");
  }
}

void handleNavigateState() {
  setStatusIndicator(1); // 导航：黄灯
  if (isEmptyQueue()) {
    currentState = STATE_IDLE;
    return;
  }
  PickTask task = taskQueue[0];
  Serial.printf("水平导航至货架%d层\n", task.shelfId);
  analogWrite(NAV_MOTOR, 180);
  // 导航完成：到达货架底部
  if (millis() - stateStartTime > 2000) {
    currentState = STATE_ELEVATING;
    subState = SUB_STATE_VERTICAL;
    stateStartTime = millis();
    displayState("ELEV");
  }
}

void handleElevatingState() {
  setStatusIndicator(2); // 提升中：闪烁黄灯
  PickTask task = taskQueue[0];
  uint16_t targetHeight = task.shelfId * 100; // 目标高度=层数×100cm
  uint16_t currentHeight = analogRead(HEIGHT_ENCODER);
  Serial.printf("提升至高度%dcm，当前%dcm\n", targetHeight, currentHeight);
  
  switch (subState) {
    case SUB_STATE_VERTICAL:
      analogWrite(ELEVATOR_MOTOR, 150); // 启动提升
      if (currentHeight >= targetHeight - 5 && currentHeight <= targetHeight + 5) {
        subState = SUB_STATE_ALIGN;
        analogWrite(ELEVATOR_MOTOR, 0);
        Serial.println("高度对准完成");
      }
      break;
    case SUB_STATE_ALIGN:
      // 微调对准
      analogWrite(ELEVATOR_MOTOR, currentHeight < targetHeight ? 50 : -50);
      if (abs(currentHeight - targetHeight) < 2) {
        subState = SUB_STATE_HORIZONTAL;
        analogWrite(ELEVATOR_MOTOR, 0);
        currentState = STATE_PICKING;
        stateStartTime = millis();
        displayState("PICK");
      }
      break;
  }
}

void handlePickingState() {
  setStatusIndicator(1); // 拣货：黄灯
  if (isEmptyQueue()) {
    currentState = STATE_IDLE;
    return;
  }
  PickTask task = dequeueTask();
  Serial.printf("高位拣取货物%d\n", task.itemId);
  analogWrite(GRAB_MOTOR, 200); // 抓取动作
  if (millis() - stateStartTime > 1500) {
    analogWrite(GRAB_MOTOR, 0);
    currentState = STATE_RETURNING;
    stateStartTime = millis();
    displayState("RETURN");
  }
}

void handleReturningState() {
  setStatusIndicator(0); // 返回：绿灯
  Serial.println("返回立体库入口");
  analogWrite(NAV_MOTOR, -180);
  analogWrite(ELEVATOR_MOTOR, -150); // 下降至地面
  if (millis() - stateStartTime > 4000) {
    analogWrite(NAV_MOTOR, 0);
    analogWrite(ELEVATOR_MOTOR, 0);
    currentState = STATE_IDLE;
    displayState("IDLE");
  }
}

void handleFaultState() {
  setStatusIndicator(3); // 故障：红灯
  displayState("FAULT");
  analogWrite(NAV_MOTOR, 0);
  analogWrite(ELEVATOR_MOTOR, 0);
  analogWrite(GRAB_MOTOR, 0);
  if (millis() - stateStartTime > 3000) {
    currentState = STATE_IDLE;
    displayState("IDLE");
  }
}

// --- 状态指示函数 ---
void setStatusIndicator(int mode) {
  // 简化：通过ESP32的LED引脚指示（实际可扩展RGB）
  switch (mode) {
    case 0: digitalWrite(23, LOW); digitalWrite(24, HIGH); digitalWrite(25, LOW); break; // 绿灯
    case 1: digitalWrite(23, HIGH); digitalWrite(24, LOW); digitalWrite(25, LOW); break; // 黄灯
    case 2: digitalWrite(23, HIGH); digitalWrite(24, HIGH); digitalWrite(25, LOW); break; // 闪烁黄灯（简化为常亮）
    case 3: digitalWrite(23, LOW); digitalWrite(24, LOW); digitalWrite(25, HIGH); break; // 红灯
  }
}

void displayState(const char* stateStr) {
  display.clear();
  display.showNumberDec(currentState, false); // 显示状态编号
  display.setBrightness(5);
}

// --- 模拟传感器与电机 ---
uint16_t getHeightSensor() {
  // 模拟高度编码器：返回0-1000的数值
  static uint16_t h = 0;
  if (currentState == STATE_ELEVATING) h += 2;
  else if (currentState == STATE_RETURNING && subState == SUB_STATE_VERTICAL) h -= 3;
  return h;
}

void setup() {
  Serial.begin(115200);
  pinMode(NAV_MOTOR, OUTPUT);
  pinMode(ELEVATOR_MOTOR, OUTPUT);
  pinMode(GRAB_MOTOR, OUTPUT);
  pinMode(PRIORITY_BUTTON, INPUT_PULLUP);
  pinMode(23, OUTPUT); pinMode(24, OUTPUT); pinMode(25, OUTPUT);
  // 初始化数码管
  display.setBrightness(5);
  // 添加初始任务
  enqueueTask({3, 201, 2, 1});
  enqueueTask({5, 202, 1, 2});
  Serial.println("立体库拣货初始化完成");
}

void loop() {
  // 高度传感器更新
  // 故障检测（简化）
  if (getHeightSensor() > 1000 || getHeightSensor() < 0) {
    currentState = STATE_FAULT;
    stateStartTime = millis();
  }
  // FSM状态处理
  switch (currentState) {
    case STATE_IDLE: handleIdleState(); break;
    case STATE_NAVIGATE: handleNavigateState(); break;
    case STATE_ELEVATING: handleElevatingState(); break;
    case STATE_PICKING: handlePickingState(); break;
    case STATE_RETURNING: handleReturningState(); break;
    case STATE_FAULT: handleFaultState(); break;
  }
  delay(50);
}

6、应急越障拣货机器人——FSM中断响应+任务中断队列+多模态指示
适用场景：仓储火灾、局部障碍等应急场景下的拣货，要求机器人在执行常规任务时，能响应紧急任务中断，自主越障后恢复任务，同时通过多模态指示（声光+数码管）实时反馈状态，便于应急指挥。

核心设计：
FSM中断响应机制：支持紧急任务触发状态中断，执行越障子状态后，恢复原任务；
任务中断队列：存储被中断的常规任务，越障完成后自动恢复队列处理；
多模态状态指示：结合LED闪烁、蜂鸣器频率、数码管文字显示，区分常规、应急、越障状态。

/* 应急越障拣货机器人：FSM中断响应+任务中断队列+多模态指示
   核心逻辑：紧急任务触发FSM中断→执行越障→恢复原任务 + 中断任务队列 + 声光+数码管指示
   硬件：Arduino Mega + 3路BLDC（导航/越障/抓取）+ 障碍传感器 + 紧急任务红外传感器 + 多模态指示模块
   状态参数：越障速度3，越障高度15cm，紧急任务优先级最高
*/

// --- FSM状态定义（含中断、越障子状态） ---
typedef enum {
  STATE_IDLE = 0,
  STATE_NORMAL_TASK,   // 执行常规任务
  STATE_INTERRUPT,     // 紧急任务中断
  STATE_OBSTACLE,      // 越障状态
  STATE_FAULT
} FSMState;

// 越障子状态
typedef enum {
  SUB_OBSTACLE_DETECT = 0, // 障碍检测
  SUB_OBSTACLE_CLIMB,      // 越障动作
  SUB_OBSTACLE_RESUME      // 恢复原路径
} ObstacleSubState;

FSMState currentState = STATE_IDLE;
ObstacleSubState obsSubState = SUB_OBSTACLE_DETECT;
unsigned long stateStartTime = 0;

// --- 任务队列（常规+中断任务） ---
struct PickTask {
  uint8_t shelfId;
  uint8_t itemId;
  uint8_t qty;
  bool isEmergency; // 是否紧急任务
};
#define TASK_QUEUE_SIZE 12
PickTask taskQueue[TASK_QUEUE_SIZE];
uint8_t queueHead = 0, queueTail = 0, taskCount = 0;
PickTask interruptedTask; // 被中断的常规任务
bool hasInterruptedTask = false;

// --- 硬件引脚定义 ---
#define NAV_MOTOR 9
#define OBSTACLE_MOTOR 10 // 越障专用电机（抬升轮）
#define GRAB_MOTOR 11
#define EMERGENCY_SENSOR 12 // 紧急任务红外传感器
#define OBSTACLE_SENSOR 13  // 障碍检测传感器
#define SEGMENT_A 14, SEGMENT_B 15, SEGMENT_C 16, SEGMENT_D 17
#define SEGMENT_E 18, SEGMENT_F 19, SEGMENT_G 20, DP 21
#define BUZZER 22
#define STATUS_LED 23

// --- 任务队列操作 ---
bool enqueueTask(PickTask task) {
  if ((queueTail + 1) % TASK_QUEUE_SIZE == queueHead) {
    Serial.println("任务队列已满！");
    return false;
  }
  taskQueue[queueTail] = task;
  queueTail = (queueTail + 1) % TASK_QUEUE_SIZE;
  taskCount++;
  return true;
}

PickTask dequeueTask() {
  if (taskCount == 0) return {0,0,0,false};
  PickTask task = taskQueue[queueHead];
  queueHead = (queueHead + 1) % TASK_QUEUE_SIZE;
  taskCount--;
  return task;
}

bool isEmptyQueue() { return taskCount == 0; }

// --- FSM状态处理（含中断机制） ---
void handleIdleState() {
  setMultiIndicator(0, "IDLE"); // 待机：绿灯常亮，数码管显示IDLE
  if (!isEmptyQueue()) {
    currentState = STATE_NORMAL_TASK;
    stateStartTime = millis();
  }
  // 检测紧急任务触发
  if (digitalRead(EMERGENCY_SENSOR) == HIGH) {
    // 紧急任务入队（最高优先级）
    enqueueTask({9, 301, 1, true});
    setMultiIndicator(3, "EMERGENCY!"); // 红灯闪烁，蜂鸣长鸣
    Serial.println("紧急任务触发！");
  }
}

void handleNormalTaskState() {
  setMultiIndicator(1, "TASK"); // 常规任务：黄灯，数码管显示TASK
  PickTask task = dequeueTask();
  if (task.isEmergency) {
    // 中断当前常规任务，执行紧急任务
    currentState = STATE_INTERRUPT;
    hasInterruptedTask = true;
    interruptedTask = task; // 保存被中断的任务（实际应保存原任务，简化为记录）
    stateStartTime = millis();
    setMultiIndicator(2, "INTERRUPT");
    return;
  }
  Serial.printf("执行常规任务：拣取货架%d货物\n", task.shelfId);
  // 模拟常规任务动作
  analogWrite(NAV_MOTOR, 150);
  if (millis() - stateStartTime > 3000) {
    // 任务完成，返回待机
    analogWrite(NAV_MOTOR, 0);
    currentState = STATE_IDLE;
  }
}

void handleInterruptState() {
  setMultiIndicator(2, "INTERRUPT");
  // 检测障碍物
  if (digitalRead(OBSTACLE_SENSOR) == HIGH) {
    currentState = STATE_OBSTACLE;
    obsSubState = SUB_OBSTACLE_DETECT;
    stateStartTime = millis();
    setMultiIndicator(4, "OBSTACLE"); // 越障：蓝灯闪烁，蜂鸣急促
    return;
  }
  // 无障碍，直接执行紧急任务
  executeEmergencyTask();
}

void handleObstacleState() {
  switch (obsSubState) {
    case SUB_OBSTACLE_DETECT:
      Serial.println("检测障碍物，准备越障");
      analogWrite(NAV_MOTOR, 0);
      obsSubState = SUB_OBSTACLE_CLIMB;
      stateStartTime = millis();
      break;
    case SUB_OBSTACLE_CLIMB:
      // 执行越障：抬升轮+越障电机联动
      analogWrite(OBSTACLE_MOTOR, 200); // 抬升越障轮
      analogWrite(NAV_MOTOR, 100); // 低速前进越障
      Serial.println("执行越障动作");
      if (millis() - stateStartTime > 4000) {
        obsSubState = SUB_OBSTACLE_RESUME;
        analogWrite(OBSTACLE_MOTOR, 0);
        stateStartTime = millis();
      }
      break;
    case SUB_OBSTACLE_RESUME:
      // 越障完成，恢复路径
      analogWrite(NAV_MOTOR, 150);
      Serial.println("越障完成，恢复紧急任务");
      if (millis() - stateStartTime > 2000) {
        obsSubState = SUB_OBSTACLE_DETECT;
        currentState = STATE_INTERRUPT;
        stateStartTime = millis();
        setMultiIndicator(2, "INTERRUPT");
      }
      break;
  }
}

void executeEmergencyTask() {
  if (isEmptyQueue()) {
    // 紧急任务完成后，恢复被中断的常规任务
    if (hasInterruptedTask) {
      enqueueTask(interruptedTask);
      hasInterruptedTask = false;
    }
    currentState = STATE_IDLE;
    return;
  }
  PickTask task = dequeueTask();
  Serial.printf("执行紧急任务：拣取货架%d货物\n", task.shelfId);
  analogWrite(NAV_MOTOR, 200); // 高速前往紧急任务点
  if (millis() - stateStartTime > 3000) {
    analogWrite(NAV_MOTOR, 0);
    if (hasInterruptedTask) {
      enqueueTask(interruptedTask);
      hasInterruptedTask = false;
    }
    currentState = STATE_IDLE;
    setMultiIndicator(0, "IDLE");
  }
}

void handleFaultState() {
  setMultiIndicator(5, "FAULT"); // 故障：红灯常亮，蜂鸣连续
  analogWrite(NAV_MOTOR, 0);
  analogWrite(OBSTACLE_MOTOR, 0);
  if (millis() - stateStartTime > 3000) {
    currentState = STATE_IDLE;
    setMultiIndicator(0, "IDLE");
  }
}

// --- 多模态状态指示 ---
void setMultiIndicator(int mode, const char* text) {
  switch (mode) {
    case 0: // 待机：绿灯常亮，蜂鸣短鸣1次
      digitalWrite(STATUS_LED, LOW);
      beep(100, 50);
      break;
    case 1: // 常规任务：黄灯常亮，蜂鸣无
      digitalWrite(STATUS_LED, HIGH);
      noBeep();
      break;
    case 2: // 中断：黄灯闪烁，蜂鸣短鸣2次
      digitalWrite(STATUS_LED, !digitalRead(STATUS_LED));
      beep(100, 50); beep(100, 50);
      break;
    case 3: // 紧急任务触发：红灯闪烁，蜂鸣长鸣
      digitalWrite(STATUS_LED, HIGH);
      beep(500, 0);
      break;
    case 4: // 越障：蓝灯闪烁（实际用RGB扩展），蜂鸣急促
      for (int i = 0; i < 5; i++) { beep(100, 0); }
      break;
    case 5: // 故障：红灯常亮，蜂鸣连续
      digitalWrite(STATUS_LED, HIGH);
      for (int i = 0; i < 10; i++) { beep(200, 0); }
      break;
  }
  // 数码管显示文字（简化：显示状态编号）
  displayText(text);
}

void displayText(const char* text) {
  // 简化：7段数码管显示文字首字母（实际可扩展为点阵屏）
  // 此处省略具体数码管驱动，需结合硬件扩展
}

void beep(int duration, int interval) {
  digitalWrite(BUZZER, HIGH);
  delay(duration);
  digitalWrite(BUZZER, LOW);
  if (interval > 0) delay(interval);
}

void noBeep() { digitalWrite(BUZZER, LOW); }

// --- 初始化与主循环 ---
void setup() {
  Serial.begin(115200);
  pinMode(NAV_MOTOR, OUTPUT);
  pinMode(OBSTACLE_MOTOR, OUTPUT);
  pinMode(GRAB_MOTOR, OUTPUT);
  pinMode(EMERGENCY_SENSOR, INPUT_PULLUP);
  pinMode(OBSTACLE_SENSOR, INPUT_PULLUP);
  pinMode(STATUS_LED, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  // 初始化数码管（简化）
  initSegmentDisplay();
  // 添加常规任务
  enqueueTask({1, 302, 2, false});
  enqueueTask({2, 303, 3, false});
  Serial.println("应急拣货机器人初始化完成");
}

void loop() {
  // 障碍检测与紧急任务检测（实时响应）
  if (digitalRead(EMERGENCY_SENSOR) == HIGH && currentState == STATE_NORMAL_TASK) {
    enqueueTask({9, 301, 1, true});
    setMultiIndicator(3, "EMERGENCY!");
  }
  if (digitalRead(OBSTACLE_SENSOR) == HIGH && (currentState == STATE_INTERRUPT || currentState == STATE_NORMAL_TASK)) {
    if (currentState != STATE_OBSTACLE) {
      currentState = STATE_OBSTACLE;
      obsSubState = SUB_OBSTACLE_DETECT;
      stateStartTime = millis();
    }
  }
  // FSM状态处理
  switch (currentState) {
    case STATE_IDLE: handleIdleState(); break;
    case STATE_NORMAL_TASK: handleNormalTaskState(); break;
    case STATE_INTERRUPT: handleInterruptState(); break;
    case STATE_OBSTACLE: handleObstacleState(); break;
    case STATE_FAULT: handleFaultState(); break;
  }
  delay(50);
}

// 简化的数码管初始化
void initSegmentDisplay() {
  pinMode(SEGMENT_A, OUTPUT);
  pinMode(SEGMENT_B, OUTPUT);
  pinMode(SEGMENT_C, OUTPUT);
  pinMode(SEGMENT_D, OUTPUT);
  pinMode(SEGMENT_E, OUTPUT);
  pinMode(SEGMENT_F, OUTPUT);
  pinMode(SEGMENT_G, OUTPUT);
  pinMode(DP, OUTPUT);
  // 初始清屏
  digitalWrite(SEGMENT_A, LOW);
  digitalWrite(SEGMENT_B, LOW);
  // ... 其他段同理
}

要点解读

1. FSM驱动：拣货机器人的“行为骨架”，实现状态可控与流程闭环
   FSM（有限状态机）是智能仓储拣货机器人的核心控制逻辑，通过明确的状态划分和切换规则，实现全流程行为的精准可控，是应对复杂仓储流程的核心保障：
   状态划分需覆盖全流程：从待机、导航、拣货到返回、故障，所有关键动作必须对应独立状态，确保每个流程节点有明确的行为边界，避免逻辑混乱；案例4的5状态设计覆盖平库拣货全流程，案例5通过子状态实现立体库提升的精细化控制，案例6通过中断机制实现应急场景的状态切换；
   状态切换依赖明确触发条件：切换必须基于传感器信号、任务完成、超时等可量化条件，而非随机跳转，确保流程的严谨性；例如平库机器人从导航到拣货的触发条件是“到达货架位置（时间/传感器判断）”，而非固定延时；
   资源管控与故障隔离：FSM需对硬件资源（电机、传感器）按状态进行管控，仅在当前状态下启用相关硬件；故障状态必须独立隔离，优先停止所有电机，保障设备安全，避免故障蔓延。
2. 任务队列：拣货任务的“调度中枢”，保障多任务有序执行与优先级管理
   任务队列是多任务调度的核心，解决仓储机器人面临的多订单、多优先级任务处理需求，通过队列化管理实现任务的有序调度与灵活适配：
   队列结构适配场景需求：平库常规拣货采用循环队列（案例4），支持批量任务顺序处理，避免空间浪费；立体库和应急场景采用优先级队列（案例5、6），确保紧急任务、高位任务优先执行，满足业务优先级需求；
   队列操作需边界安全：入队、出队必须判断队列空/满边界，避免数组越界；案例1通过队列头尾指针和任务计数实现循环队列，案例2通过排序算法实现优先级队列，核心是保障数据操作的安全性与可靠性；
   任务队列与FSM深度协同：FSM的状态切换依赖队列状态（空/非空），而队列的任务流转依赖FSM的处理进度，形成“队列驱动状态、状态消费任务”的闭环，确保任务不丢失、流程不中断；例如案例1中待机状态检测队列非空即切换至导航，导航完成后消费队列中的任务。
3. 状态指示：人机交互的“信息窗口”，实现状态可视化与故障快速定位
   状态指示是拣货机器人的必备功能，通过直观的声光、数码管等多模态反馈，让操作人员实时掌握机器人状态，快速响应异常，是提升运维效率的核心：
   指示维度贴合业务需求：不同状态需对应差异化的反馈方式，让操作人员一眼识别状态类型；案例1用绿/黄/红LED区分待机、任务、故障，案例2用数码管显示货架层数，案例3用蜂鸣频率区分应急与越障，核心是让状态信息可视化、可感知；
   指示需与FSM状态严格同步：状态指示的更新必须与FSM的状态切换同步，确保指示信息的真实性，避免误导操作人员；例如故障状态时必须同步切换为红灯和长鸣蜂鸣器，且仅在故障恢复后切换回待机指示；
   指示设计兼顾简洁与冗余：在复杂场景下，单一指示方式可能失效，需设计冗余指示（如声光组合、数码管+LED组合），确保在噪音、强光环境下仍能传递信息；案例3采用声光+数码管的多模态指示，兼顾不同环境的信息传递需求。
4. BLDC执行机构：状态落地的“动力引擎”，匹配FSM的动态执行需求
   BLDC电机作为核心执行机构，直接决定FSM状态意图的落地效果，需与FSM的动态切换深度适配，满足拣货机器人的速度、扭矩、精度需求：
   控制模式适配状态动作：不同状态下BLDC电机需采用对应控制模式，实现精准动作；例如导航状态采用速度模式实现快速移动，拣货状态采用位置模式实现精准抓取，越障状态采用扭矩模式提供大驱动力，确保动作与状态匹配；
   动态响应匹配状态切换节奏：FSM状态切换具有实时性要求，BLDC需具备毫秒级的动态响应速度，在状态切换时快速调整输出，避免因电机响应滞后导致流程卡顿；例如应急场景中越障电机需在状态切换后立即启动，快速完成抬升动作；
   闭环控制保障动作精度：BLDC必须结合编码器实现闭环控制，精准跟踪FSM输出的目标轨迹，避免开环控制的误差累积；例如立体库提升时，编码器反馈高度位置，结合PID算法实现精准的层高定位，确保提升到位后误差在允许范围内；
   安全保护适配故障状态：BLDC需具备过流、过压、堵转保护机制，当FSM切换至故障状态时，电机能立即停止并触发保护，避免硬件损坏，保障设备安全。
5. 工程落地：FSM、队列、指示的协同优化，兼顾稳定性与扩展性
   工程落地的核心是实现FSM、任务队列、状态指示的高效协同，在资源有限的Arduino平台上兼顾系统稳定性、可维护性与功能扩展性：
   代码模块化降低耦合度：将FSM状态处理、队列操作、状态指示、BLDC控制封装为独立模块，通过接口通信，降低模块间的耦合度；例如案例中把任务队列操作独立为函数，FSM通过调用接口实现队列交互，便于后续维护和功能扩展；
   资源分配适配硬件约束：Arduino平台资源有限，需合理分配引脚、算力、内存资源；例如使用ESP32或Mega等高性能平台应对多电机控制和队列运算，避免8位平台算力不足；PWM通道按电机数量合理分配，避免资源冲突；
   调试机制保障问题定位：代码需内置串口调试信息，记录状态切换、任务队列变化、电机动作等关键信息，便于快速定位问题；同时设计仿真模式，通过模拟传感器信号验证FSM和队列逻辑，降低调试成本；
   扩展性预留适配需求变化：架构设计需预留扩展接口，例如支持新增FSM状态、扩展任务队列容量、新增指示方式；例如案例2预留数码管扩展接口，案例3预留多模态指示接口，为后续新增仓储功能提供扩展空间。

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