Arduino BLDC园林机器人基于距离触发的避障与喷洒控制算法，是以Arduino/ESP32为控制核心、BLDC无刷电机为底盘驱动、超声波/ToF等距离传感器为感知源，通过距离分区阈值触发状态机，实现避障优先、喷洒联动与平滑运动切换的智能园林作业方案。 该方案具备距离分区感知与风险分级响应、避障优先与喷洒联动、BLDC平滑运动控制、状态机驱动多模式切换四大特点，主要应用于公园绿篱修剪与喷洒、果园行间植保、智能草坪养护及科研教学等场景；实际部署时需重点关注传感器选型与抗干扰、主控算力与实时性、BLDC驱动与FOC控制、电源隔离与EMC防护、安全机制及环境适应性。
一、 技术架构与主要特点
距离分区感知与风险分级响应：区别于"单一阈值避障"（如仅检测前方30cm内障碍），该方案通过"空间分区+风险分级"实现精细化环境认知。以机器人为中心，按距离划分为三级区域：近距危险区（如0_{15cm，侵入即触发紧急制动）、中距预警区（如15}40cm，检测到障碍则启动路径重规划）、远距安全区（如40~80cm以外，无障时维持原路径）。不同分区匹配不同传感器，近距用红外或ToF传感器（高精度），中远距用超声波（覆盖广），兼顾精度与效率。
避障优先与喷洒联动：系统遵循"避障为生存、喷洒为任务"的原则。当距离传感器检测到障碍物侵入预警区时，立即挂起喷洒任务并触发避障动作（减速→转向→绕行）；障碍物清除后自动恢复喷洒。喷洒泵（水泵或电磁阀）的启停由距离阈值和作业状态联合控制：仅在安全区内且处于"喷洒"状态时才开启，避免在避障或转弯时误喷造成药液浪费。
BLDC平滑运动控制：采用FOC（磁场定向控制）驱动BLDC电机，通过正弦电流驱动消除传统六步换相的转矩纹波，实现低速平滑运转和低噪音输出。避障过程中的减速、转向、加速均通过S形速度曲线（S-curve）规划，使加速度连续无突变，避免药液因急停急启而飞溅不均。
状态机驱动多模式切换：系统内置有限状态机（FSM），定义"喷洒直行"“避障绕行”“减速过渡”“紧急停机"等离散状态。距离传感器数据作为状态转移触发条件，Arduino端执行状态转移逻辑与电机控制。状态切换时对电机速度做缓变处理，确保从高速喷洒到避障停机的过渡平滑无冲击。
二、 典型应用场景
公园绿篱修剪与喷洒：园林机器人在绿篱行间自主行驶，前方超声波检测绿篱边界距离，当距离小于设定阈值时自动调整喷洒角度和流量；侧方传感器检测行人或设施，触发避障绕行。BLDC低噪音特性适合公园等对噪声敏感的环境。
果园行间植保喷洒：机器人在果树行间穿梭，前方ToF传感器检测果树树干距离，进入喷洒有效范围后自动开启水泵；当距离过近（如<20cm）时判定为树干，立即停机避障防止碰撞。多路超声波矩阵可构建环境感知，模糊逻辑驱动BLDC转向避障。
智能草坪养护：机器人在草坪上按预设路径行驶喷洒肥料或除草剂，超声波检测草坪边界（如路沿石、花坛边缘），触发减速和转向。结合IMU姿态校正，确保在坡道上喷洒均匀。
教学与科研验证平台：成本远低于商用园林机器人，适合高校和职业院校用于"传感器融合”“状态机设计”"运动控制"等课程的教学实训，也可用于RoboMaster等机器人竞赛中的避障与作业协同算法验证。
三、 关键注意事项
传感器选型与抗干扰：
超声波局限性：HC-SR04等超声波传感器在草地、灌木等吸声环境中测距精度下降，且存在盲区（通常<2cm无法检测）。建议近距补充ToF传感器（如VL53L0X，精度±3mm，盲区<1cm）。
多路协同：至少部署3路超声波（正前、左前45°、右前45°），构建前方扇形感知区域，避免单路传感器的"死角"问题。
数据滤波：超声波原始数据存在跳变，必须加入中值滤波或滑动平均滤波，过滤异常值后再送入状态机判断。
主控算力与实时性保障：多路传感器采集、状态机逻辑与FOC控制对算力要求较高。标准Arduino Uno（16MHz）难以胜任，建议采用ESP32（双核240MHz）或STM32等高算力板卡。控制回路必须使用硬件定时器中断或非阻塞定时（millis()），严禁使用delay()函数，确保控制频率≥50Hz。
BLDC驱动与FOC控制：BLDC驱动必须支持FOC（如SimpleFOC库），无FOC则低速抖动、扭矩不足，影响喷洒均匀性。编码器闭环可进一步提升低速稳定性，确保在低速度喷洒时底盘不抖动。
电源隔离与EMC防护：BLDC电机启停时电流冲击极大，严禁与Arduino及传感器共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容（1000~4700μF）吸收反电动势。动力线与信号线必须分开走线，传感器信号线需使用屏蔽双绞线。
安全机制必须完善：
紧急制动：近距危险区（如<15cm）触发立即停机，不依赖软件响应延迟。
硬件急停：通过急停按钮直接切断电机驱动电源。
通讯超时保护：与上位机的通讯中断超过设定时间（如5秒），自动进入安全停机模式。
速度/加速度限幅：在软件中对目标值做constrain()限幅，避免急启急停对机械结构的冲击。
环境适应性设计：
防水密封：园林机器人工作在潮湿环境中，传感器和电子舱需达到IP65以上防护等级。
防堵塞设计：喷洒管路需加装过滤器，防止草屑、泥沙堵塞喷嘴。
地形适应：草地、泥地等松软地面可能导致车轮打滑，需结合IMU检测实际运动状态，当检测到打滑时降低速度并调整喷洒策略。

1、多路超声波矩阵避障与状态机喷洒
场景：机器人在复杂、非结构化的园林或农田中自主穿梭，需要对来自不同方向（前、左、右、左前、右前）的障碍物做出灵活响应，并联动控制喷洒系统的启停。

核心逻辑：通过布置在机器人前部及两侧的多个超声波传感器构成感知矩阵，实时测量与周围障碍物的距离。根据距离远近，划分出“紧急区”、“警告区”和“安全区”，并据此驱动一台状态机，在“喷洒”、“避障”和“返回”等状态间切换，同时控制BLDC电机的差速转向和喷洒泵的开闭。

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

// 定义BLDC差速电机 (motorL, motorR)
// 定义5路超声波传感器 (sonarF, sonarL, sonarR, sonarFL, sonarFR)
// 定义喷洒泵控制引脚 (PUMP_PIN)

// 安全距离阈值 (单位: cm)
const int EMERGENCY_ZONE = 15;
const int WARNING_ZONE = 40;
const int SAFE_ZONE = 80;

// 机器人状态
enum RobotState { SPRAYING, AVOIDING, RETURNING };
RobotState state = SPRAYING;

void loop() {
  // 1. 多路超声波数据采集与过滤
  int dF = sonarF.ping_cm(); // 前方距离
  int dL = sonarL.ping_cm(); // 左方距离
  int dR = sonarR.ping_cm(); // 右方距离
  // ... 对无效数据(0或超出范围)进行过滤，设为最大值

  // 2. 状态机决策与电机/喷洒控制
  switch (state) {
    case SPRAYING:
      // 检测到前方距离过近则切换至避障状态
      if (dF < WARNING_ZONE) {
        state = AVOIDING;
        digitalWrite(PUMP_PIN, LOW); // 停止喷洒
        break;
      }
      // 正常喷洒: 直线前进，适当调节速度
      motorL.move(targetSpeed);
      motorR.move(targetSpeed);
      digitalWrite(PUMP_PIN, HIGH); // 开启喷洒
      break;

    case AVOIDING:
      // 差速转向避障逻辑 (如: 前方有障碍，优先右转)
      if (dF < EMERGENCY_ZONE) { // 紧急倒车
        motorL.move(-slowSpeed);
        motorR.move(-slowSpeed);
      } else if (dL > dR) { // 左侧更开阔，左转
        motorL.move(-turnSpeed);
        motorR.move(turnSpeed);
      } else { // 否则右转
        motorL.move(turnSpeed);
        motorR.move(-turnSpeed);
      }
      // 持续检测，直到前方安全，切回喷洒状态
      if (dF > SAFE_ZONE) {
        state = SPRAYING;
      }
      break;
    // ... 其他状态 (如RETURNING)
  }
  delay(30);
}

2、基于模糊逻辑的避障与动态调速喷洒
场景：在草坪、果园等障碍物分布不规则的环境中，机器人不仅需要避开障碍，还要根据行进速度动态调整喷洒量，以保证施药均匀性。

核心逻辑：将多个超声波传感器的距离数据作为模糊逻辑控制器的输入，输出平滑的转向和速度指令，替代生硬的“遇障即转”策略。同时，通过编码器反馈的实际车速，动态调整喷洒泵的开启时间或流量，实现“车速快，喷量多；车速慢，喷量少”的变量喷洒。

// 假设已定义BLDC电机、编码器、超声波传感器和喷洒泵

// 模糊控制输入：前方距离(dF)、左侧距离(dL)、右侧距离(dR)
// 模糊控制输出：转向指令(turnCmd)、速度指令(speedCmd) (范围归一化)
float turnCmd, speedCmd;

// 喷洒控制参数
int baseSprayDuration = 300; // 基础喷洒脉冲时间(ms)
float currentSpeed = 0;      // 从编码器获取的当前速度

void loop() {
  // 1. 采集距离数据 (同案例一)
  // ...

  // 2. 模糊逻辑决策 (简化示例: 仅以距离判断转向趋势)
  if (dF < 30) { // 前方太近
    speedCmd = 0.2; // 减速
    if (dL > dR) turnCmd = -0.6; // 左侧更空，左转
    else turnCmd = 0.6;
  } else if (dF < 60) {
    speedCmd = 0.6;
    turnCmd = (dL - dR) * 0.02; // 根据左右距离差微调方向
  } else {
    speedCmd = 1.0; // 前方开阔，全速
    turnCmd = 0;
  }
  // 实际应用中，此处应实现更完整的模糊推理规则表

  // 3. 执行运动指令 (速度与转向混合)
  motorL.move(speedCmd - turnCmd);
  motorR.move(speedCmd + turnCmd);

  // 4. 动态喷洒控制
  // 从编码器更新当前速度 currentSpeed (单位: 米/秒)
  // 速度越快，单次喷洒脉冲越长，保持单位面积施药量恒定
  int dynamicDuration = baseSprayDuration * (targetSpeed / max(0.1, currentSpeed));
  // 每隔一定距离或时间触发一次喷洒脉冲
  if (/* 条件满足 */) {
    digitalWrite(PUMP_PIN, HIGH);
    delay(dynamicDuration);
    digitalWrite(PUMP_PIN, LOW);
  }
  delay(20);
}

3、全局路径规划下的避障与定点喷洒
场景：机器人需要在较大面积的园林中，按照预设的GPS航点或作业地图进行自动巡检和喷洒作业。

核心逻辑：系统分为上位机（如手机APP或树莓派）和下位机（Arduino）。上位机负责全局路径规划，生成一系列目标航点（GPS坐标）或参考路径。Arduino通过GPS模块实时定位，并结合IMU校正航向，控制BLDC电机跟踪路径点。在跟踪过程中，若局部传感器（超声波/激光雷达）检测到突发障碍，则触发避障行为；到达喷洒区域，则根据视觉或预设规则启动喷洒。

#include <SimpleFOC.h>
#include <TinyGPS++.h>
#include <Wire.h> // for IMU

// 定义BLDC差速电机、GPS模块、IMU和避障传感器(超声波/激光)

// 目标航点 (从串口接收)
float targetLat, targetLng;
const float ARRIVAL_THRESHOLD = 2.0; // 到达目标点阈值(米)

void loop() {
  // 1. 更新自身位姿
  if (gps.location.isValid()) {
    double currentLat = gps.location.lat();
    double currentLng = gps.location.lng();
    // 结合IMU融合当前航向 (yaw)
  }

  // 2. 计算到目标点的偏差
  double distToTarget = TinyGPSPlus::distanceBetween(currentLat, currentLng, targetLat, targetLng);
  double bearingToTarget = TinyGPSPlus::courseTo(currentLat, currentLng, targetLat, targetLng);
  double headingError = bearingToTarget - fusedYaw; // 归一化处理

  // 3. 局部避障检测 (优先级高于路径跟踪)
  int obstacleDist = getObstacleDistance(); // 从超声波/雷达获取
  if (obstacleDist < 50) { // 发现近距障碍物
    // 执行避障策略 (如: 减速、转向、绕行) [citation:10]
    // 此处可调用案例二中的模糊避障逻辑，或执行“右转”等固定策略
    executeObstacleAvoidance();
    return; // 跳过本次路径跟踪
  }

  // 4. 路径跟踪控制 (PID控制航向)
  float angularCmd = constrain(headingError * 1.2, -1.0, 1.0);
  float linearCmd = constrain(distToTarget * 0.5, 0.2, 1.0);
  motorL.move(linearCmd - angularCmd);
  motorR.move(linearCmd + angularCmd);

  // 5. 喷洒逻辑: 到达目标点或特定区域开启喷洒
  if (distToTarget < ARRIVAL_THRESHOLD) {
    digitalWrite(PUMP_PIN, HIGH); // 开启喷洒
    // 例如: 喷洒10秒后停止，并进入下一个航点
  }
  delay(50);
}

要点解读
多传感器融合是避障可靠性的基石：单一传感器（如仅用超声波）在面对特殊材质（如透明塑料薄膜、细铁丝网）或特定角度时可能失效。工程上通常采用“远-中-近”多层感知架构，例如：远距离使用激光雷达或视觉构建地图，中近距离依靠超声波阵列，近距离辅以红外或触须传感器，实现优势互补。

模糊逻辑比硬阈值更适合“非结构化”环境：在农田、果园等环境中，障碍物距离是一个连续变化量。使用if (dist < 30)这样的硬阈值会导致机器人动作生硬、顿挫。将距离模糊化为“很近”、“较近”、“安全”等概念，再通过模糊规则表推理出“慢速左转”、“快速右转”等平滑指令，能使机器人的行为更像人类驾驶，更适应复杂地形。

“松耦合”任务分解是工程实践的良好模式：即将全局导航（如GPS路径跟踪）与局部避障（如超声波避障）视为两个相对独立的模块。Arduino主循环可以优先处理高频的避障传感器数据，确保安全；而对GPS这种低频、大滞后数据的处理，则可以作为“宏观目标”输入，影响机器人的长期行为。这种松耦合设计逻辑清晰，易于调试。

喷洒量与车速联动是实现精准农业的关键：固定流量的喷洒会导致机器人在转弯或避障减速时，单位面积施药量过大，造成浪费和污染。通过编码器获取实时车速，动态调整喷洒泵的PWM占空比或电磁阀的开启时间（车速越快，喷量越大），是实现“变量喷洒”、保证施药均匀性的核心算法。

BLDC的力矩模式是实现柔和启停与节能的保障：相比于有刷电机或简易PWM调速，BLDC电机配合磁场定向控制（FOC）能实现更平滑的力矩输出。这对于避障时的“柔顺转向”、启动和停止时的“无冲击”至关重要，也能保护电机和传动机构。同时，BLDC的高效率特性在电池供电的园林机器人中能显著延长续航时间。

4、多传感器融合的智能灌溉决策与基础避障（按需触发喷洒）
适用场景：小型农田、温室大棚的定点巡检与精准灌溉，机器人需同时监测土壤墒情、植株高度与光照，仅当满足“土壤干燥+有作物+适宜光照”条件时，才触发喷洒，同时通过超声波实现基础避障，避免碰撞障碍物。

核心控制逻辑：
多源数据感知：采集土壤湿度、光照强度、植株高度（超声波估算），构建多维度环境数据；
逻辑决策触发：采用“与逻辑”判断，仅当所有条件满足时，才启动水泵喷洒，避免无效作业；
基础距离避障：通过前方超声波检测障碍物，距离低于安全阈值时，停止电机并调整路径，保障基础行驶安全；
状态联动控制：喷洒时降低行驶速度，非喷洒时按预设路径巡逻，实现作业与移动的协同。

/* ===== Arduino BLDC 智能灌溉机器人 - 多传感器决策与基础避障 =====
 * 硬件：Arduino Mega + BLDC差速底盘 + LDR + 土壤湿度传感器 + 超声波 + 水泵
 * 核心：多传感器融合决策 + 距离触发避障 + 按需喷洒
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
// 需补充Encoder和Driver初始化（根据实际硬件补全）

// ==================== 传感器引脚定义 ====================
#define SOIL_MOISTURE_PIN A0   // 土壤湿度（模拟量）
#define LDR_PIN A1             // 光照强度
#define TRIG_F 2               // 前方超声波（避障/测植株）
#define ECHO_F 3
#define PUMP_PIN 8             // 水泵控制
NewPing sonarF(TRIG_F, ECHO_F, 200);

// ==================== 决策阈值与参数 ====================
const int SOIL_DRY_THRESHOLD = 400;   // 土壤干燥阈值（值越大越干）
const int LIGHT_THRESHOLD = 500;      // 适宜光照阈值
const float MIN_PLANT_HEIGHT = 15.0;  // 最小植株高度(cm)
const int SAFE_DISTANCE = 30;         // 避障安全距离(cm)

// ==================== 状态变量 ====================
bool irrigationActive = false;
unsigned long lastIrrigationTime = 0;
const unsigned long IRRIGATION_INTERVAL = 300000; // 5分钟最小喷洒间隔

void setup() {
    Serial.begin(115200);
    
    // BLDC电机初始化
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    pinMode(PUMP_PIN, OUTPUT);
    digitalWrite(PUMP_PIN, LOW);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();

    // ==================== 1. 传感器数据采集 ====================
    int soilMoisture = analogRead(SOIL_MOISTURE_PIN);
    int lightLevel = analogRead(LDR_PIN);
    float plantHeight = sonarF.ping_cm();  // 超声波测距估算植株高度
    if (plantHeight <= 0 || plantHeight > 150) plantHeight = 0; // 过滤无效数据
    int obstacleDist = sonarF.ping_cm(); // 避障距离检测

    // ==================== 2. 【核心】避障优先：距离触发避障 ====================
    if (obstacleDist < SAFE_DISTANCE && obstacleDist > 0) {
        // 检测到障碍物，停止行驶并调整
        motorL.move(0);
        motorR.move(0);
        Serial.println("检测到障碍物，避障调整");
        delay(500);
        // 简单避障：后退后左转
        motorL.move(-0.5);
        motorR.move(0.5);
        delay(1000);
        motorL.move(0);
        motorR.move(0);
        return; // 避障优先，跳过后续决策
    }

    // ==================== 3. 【核心】多传感器决策触发喷洒 ====================
    bool needWater = (soilMoisture > SOIL_DRY_THRESHOLD);
    bool isSuitableLight = (lightLevel > LIGHT_THRESHOLD);
    bool hasCrop = (plantHeight > MIN_PLANT_HEIGHT);
    bool timeValid = (millis() - lastIrrigationTime > IRRIGATION_INTERVAL);

    if (needWater && hasCrop && isSuitableLight && timeValid) {
        // 满足所有条件，启动喷洒
        irrigationActive = true;
        lastIrrigationTime = millis();
        digitalWrite(PUMP_PIN, HIGH);
        Serial.println("启动喷洒 - 土壤干燥/有作物/光照适宜");
        
        // 喷洒期间慢速行驶，确保均匀覆盖
        motorL.move(0.3);
        motorR.move(0.3);
        delay(5000);  // 喷洒5秒
    } else {
        // 不满足条件，停止喷洒
        if (irrigationActive) {
            digitalWrite(PUMP_PIN, LOW);
            irrigationActive = false;
            Serial.println("喷洒停止");
        }
        // 无喷洒时，沿预设路径巡逻
        patrolPath();
    }

    // 调试输出
    Serial.print("土壤:"); Serial.print(soilMoisture);
    Serial.print(" 光照:"); Serial.print(lightLevel);
    Serial.print(" 植株高:"); Serial.print(plantHeight);
    Serial.print(" 障碍物:"); Serial.print(obstacleDist);
    Serial.print(" 喷洒:"); Serial.println(irrigationActive ? "ON" : "OFF");

    delay(1000);  // 1秒检测周期
}

// 预设巡逻路径（简单直行，可扩展为曲线/折线）
void patrolPath() {
    motorL.move(0.6);
    motorR.move(0.6);
}

5、超声波矩阵避障+自主导航喷洒（模糊逻辑转向）
适用场景：垄间、大棚等结构化农田的自主喷洒，机器人需沿作物行间穿行，通过前、左、右三路超声波构建局部环境感知，结合模糊逻辑判断转向方向，实现精准避障与路径修正，同时按需启动喷洒，适配非规则障碍物（如土块、杂草）。

核心控制逻辑：
多层距离感知：设置“紧急区、预警区、安全区”三级距离阈值，不同区域触发不同控制动作，兼顾安全与效率；
模糊逻辑决策：结合左右超声波与红外传感器数据，判断左右空间的空旷程度，通过模糊规则输出转向指令，避免硬逻辑转向的局限性；
硬件中断急停：前方障碍物进入紧急区时，触发硬件中断强制停止电机，实现毫秒级响应，保障极端情况的安全；
喷洒与避障协同：避障时暂停喷洒，避障完成后恢复喷洒，避免喷洒到障碍物或漏喷作物。

/* ===== 多传感器融合避障 + 超声波矩阵自主导航喷洒 =====
 * 硬件：Arduino/ESP32 + 2×BLDC差速电机 + 前/左/右超声波 + 左右红外 + 水泵
 * 核心：三层距离区域（紧急/预警/安全）+ 模糊转向决策 + 硬件中断急停 + 喷洒联动
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
// 需补充Encoder和Driver初始化

// ==================== 超声波与红外传感器 ====================
#define TRIG_F 2  // 前方超声波
#define ECHO_F 3
#define TRIG_L 4  // 左侧超声波
#define ECHO_L 5
#define TRIG_R 6  // 右侧超声波
#define ECHO_R 7
#define IR_LEFT A0  // 左侧红外（近距离补充检测）
#define IR_RIGHT A1 // 右侧红外
NewPing sonarF(TRIG_F, ECHO_F, 200);
NewPing sonarL(TRIG_L, ECHO_L, 150);
NewPing sonarR(TRIG_R, ECHO_R, 150);

// ==================== 距离阈值与参数 ====================
const int EMERGENCY_ZONE = 15;    // 紧急制动区(cm)
const int WARNING_ZONE = 40;      // 预警区(cm)
const int SAFE_ZONE = 80;         // 安全区(cm)
const int CROP_ROW_WIDTH = 50;    // 作物行间距参考(cm)

// ==================== 状态与变量 ====================
enum RobotState { PATH_FOLLOWING, AVOIDING, RETURNING };
RobotState state = PATH_FOLLOWING;
bool irrigationActive = false;

// ==================== 硬件中断急停 ====================
volatile bool safetyTriggered = false;

void IRAM_ATTR safetyISR() {
    // 硬件级紧急制动，绕过主循环
    motorL.move(0);
    motorR.move(0);
    safetyTriggered = true;
}

void setup() {
    Serial.begin(115200);
    
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    pinMode(IR_LEFT, INPUT);
    pinMode(IR_RIGHT, INPUT);
    
    // 前方传感器接入硬件中断（紧急区触发）
    attachInterrupt(digitalPinToInterrupt(ECHO_F), safetyISR, FALLING);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // ==================== 1. 安全中断恢复处理 ====================
    if (safetyTriggered) {
        emergencyReverse();
        safetyTriggered = false;
        return;
    }
    
    // ==================== 2. 多传感器数据采集 ====================
    int distF = sonarF.ping_cm();
    int distL = sonarL.ping_cm();
    int distR = sonarR.ping_cm();
    bool irLeft = digitalRead(IR_LEFT);
    bool irRight = digitalRead(IR_RIGHT);
    
    // 数据有效性过滤（无效数据设为最大值）
    if (distF <= 0 || distF > 150) distF = 150;
    if (distL <= 0 || distL > 150) distL = 150;
    if (distR <= 0 || distR > 150) distR = 150;
    
    // ==================== 3. 分层避障决策 ====================
    // 第一层：紧急区（硬安全，强制停止+后退）
    if (distF < EMERGENCY_ZONE && distF > 0) {
        motorL.move(0); motorR.move(0);
        emergencyReverse();
        return;
    }
    
    // 第二层：预警区（触发避障，模糊逻辑转向）
    if (distF < WARNING_ZONE) {
        state = AVOIDING;
        // 喷洒暂停（避障时不喷洒）
        if (irrigationActive) {
            digitalWrite(PUMP_PIN, LOW);
            irrigationActive = false;
        }
        
        // 【核心】模糊转向决策：判断左右空间是否空旷
        bool leftClear = (distL > SAFE_ZONE || distL <= 0) && !irLeft;
        bool rightClear = (distR > SAFE_ZONE || distR <= 0) && !irRight;
        
        if (leftClear && !rightClear) {
            // 左侧空旷，左转避障
            motorL.move(-0.5);
            motorR.move(0.5);
            Serial.println("避障：左转");
        } else if (rightClear && !leftClear) {
            // 右侧空旷，右转避障
            motorL.move(0.5);
            motorR.move(-0.5);
            Serial.println("避障：右转");
        } else if (leftClear && rightClear) {
            // 两侧都空旷，优先左转（可调整规则）
            motorL.move(-0.4);
            motorR.move(0.4);
            Serial.println("避障：左转（两侧空旷）");
        } else {
            // 两侧都狭窄，后退后尝试转向
            emergencyReverse();
            return;
        }
        delay(800); // 转向时间
        state = PATH_FOLLOWING;
        return;
    }
    
    // 第三层：安全区（正常沿行行驶+喷洒）
    if (state == PATH_FOLLOWING) {
        // 沿行行驶：根据左右距离差微调方向（保持居中）
        float distDiff = distL - distR;
        if (distDiff > 5) {
            // 左侧远，右侧近，微调右转
            motorL.move(0.6);
            motorR.move(0.5);
        } else if (distDiff < -5) {
            // 右侧远，左侧近，微调左转
            motorL.move(0.5);
            motorR.move(0.6);
        } else {
            // 居中，匀速行驶
            motorL.move(0.6);
            motorR.move(0.6);
        }
        // 安全区启动喷洒（按需持续喷洒）
        if (!irrigationActive) {
            digitalWrite(PUMP_PIN, HIGH);
            irrigationActive = true;
            Serial.println("正常行驶：启动喷洒");
        }
    }

    // 调试输出
    Serial.print("前距:"); Serial.print(distF);
    Serial.print(" 左距:"); Serial.print(distL);
    Serial.print(" 右距:"); Serial.print(distR);
    Serial.print(" 状态:"); Serial.print(state == PATH_FOLLOWING ? "行驶" : "避障");
    Serial.print(" 喷洒:"); Serial.println(irrigationActive ? "ON" : "OFF");

    delay(100); // 短周期保障实时性
}

// 紧急后退函数
void emergencyReverse() {
    motorL.move(-0.5);
    motorR.move(-0.5);
    delay(1000);
    motorL.move(0);
    motorR.move(0);
    Serial.println("紧急后退完成");
}

6、多航点巡航与动态避障喷洒（全局路径+局部重规划）
适用场景：大田、果园等大面积农田的自主喷洒，机器人需按预设航点规划全局路径，在巡航过程中实时检测障碍物，遇到障碍物时进行局部路径重规划，避开障碍后继续沿原航点行驶，同时在航点区域精准触发喷洒，实现高效全覆盖。

核心控制逻辑：
全局路径规划：预设多个航点，机器人通过GPS定位，沿航点构成的全局路径巡航，确保覆盖指定区域；
距离触发避障：巡航过程中通过超声波检测前方障碍物，距离低于阈值时暂停巡航，执行局部避障动作；
局部路径重规划：避障时根据左右空间选择转向方向，避开障碍物后，自动回归原航点路径，保障全局任务完成；
航点触发喷洒：到达预设航点时，启动喷洒装置，喷洒完成后继续前往下一航点，实现精准区域喷洒。

/* ===== Arduino BLDC 多航点巡航与动态避障喷洒 =====
 * 硬件：Arduino Mega + BLDC底盘 + GPS模块 + 超声波传感器 + 水泵
 * 核心：预设航点全局巡航 + 距离触发避障 + 局部路径重规划 + 航点触发喷洒
 */
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <SimpleFOC.h>
#include <NewPing.h>

// ==================== 硬件配置 ====================
TinyGPSPlus gps;
SoftwareSerial ss(4, 3); // GPS模块RX、TX
BLDCMotor motorL(9), motorR(10);
BLDCDriver3PWM drvL(3,5,6), drvR(11,12,13);
NewPing sonar(10, 11, 200); // 前方超声波（避障）
#define PUMP_PIN 8 // 水泵控制

// ==================== 航点与参数 ====================
const int TARGET_COUNT = 3; // 航点数量
// 预设航点坐标（示例，需根据实际农田替换）
double waypoints[TARGET_COUNT][2] = {
    {37.7749, -122.4194}, // 航点1
    {37.7750, -122.4184}, // 航点2
    {37.7760, -122.4170}  // 航点3
};
const double DISTANCE_THRESHOLD = 0.0001; // 到达航点的距离阈值（经纬度）
const int OBSTACLE_THRESHOLD = 30; // 避障距离阈值（cm）

// ==================== 状态变量 ====================
int currentWaypoint = 0; // 当前目标航点索引
bool irrigationActive = false;
bool obstacleDetected = false;

void setup() {
    Serial.begin(115200);
    ss.begin(9600);
    
    // BLDC电机初始化
    motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
    motorL.linkSensor(&dummyEnc); motorR.linkSensor(&dummyEnc);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    pinMode(PUMP_PIN, OUTPUT);
    digitalWrite(PUMP_PIN, LOW);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // ==================== 1. GPS定位数据采集 ====================
    while (ss.available() > 0) {
        gps.encode(ss.read());
    }
    
    // ==================== 2. 距离触发避障检测 ====================
    int obstacleDist = sonar.ping_cm();
    if (obstacleDist < OBSTACLE_THRESHOLD && obstacleDist > 0) {
        obstacleDetected = true;
        // 避障优先：停止巡航，执行避障
        motorL.move(0);
        motorR.move(0);
        Serial.println("检测到障碍物，启动避障");
        // 局部重规划：后退+随机转向
        motorL.move(-0.5);
        motorR.move(-0.5);
        delay(800);
        // 随机选择转向方向，增加避障灵活性
        int direction = random(0, 2);
        if (direction == 0) {
            motorL.move(-0.4);
            motorR.move(0.4); // 左转
            Serial.println("避障：左转");
        } else {
            motorL.move(0.4);
            motorR.move(-0.4); // 右转
            Serial.println("避障：右转");
        }
        delay(800);
        motorL.move(0);
        motorR.move(0);
        obstacleDetected = false;
        Serial.println("避障完成，回归航点");
        delay(500);
        return; // 避障完成后重新进入巡航逻辑
    }
    
    // ==================== 3. 航点巡航与喷洒触发 ====================
    if (gps.location.isUpdated() && !obstacleDetected) {
        double currentLat = gps.location.lat();
        double currentLng = gps.location.lng();
        
        // 检查是否到达当前航点
        if (currentWaypoint < TARGET_COUNT) {
            double targetLat = waypoints[currentWaypoint][0];
            double targetLng = waypoints[currentWaypoint][1];
            
            // 判断是否到达航点（经纬度差小于阈值）
            if (abs(currentLat - targetLat) < DISTANCE_THRESHOLD && abs(currentLng - targetLng) < DISTANCE_THRESHOLD) {
                Serial.print("到达航点: "); Serial.println(currentWaypoint + 1);
                // 到达航点，触发喷洒
                if (!irrigationActive) {
                    digitalWrite(PUMP_PIN, HIGH);
                    irrigationActive = true;
                    Serial.println("航点喷洒启动");
                }
                // 喷洒期间原地停留
                motorL.move(0);
                motorR.move(0);
                delay(5000); // 喷洒5秒
                // 喷洒完成，关闭水泵，前往下一航点
                digitalWrite(PUMP_PIN, LOW);
                irrigationActive = false;
                currentWaypoint++;
                Serial.println("航点喷洒完成，前往下一航点");
            } else {
                // 未到达航点，向目标航点方向行驶
                // 简化逻辑：直接向目标方向行驶（实际可加入航向角计算）
                motorL.move(0.6);
                motorR.move(0.6);
            }
        } else {
            // 所有航点完成，停止行驶与喷洒
            motorL.move(0);
            motorR.move(0);
            if (irrigationActive) {
                digitalWrite(PUMP_PIN, LOW);
                irrigationActive = false;
            }
            Serial.println("所有航点巡航完成，任务结束");
        }
        
        // 调试输出
        Serial.print("当前航点:"); Serial.print(currentWaypoint + 1);
        Serial.print(" 当前坐标:"); Serial.print(currentLat, 6);
        Serial.print(","); Serial.print(currentLng, 6);
        Serial.print(" 喷洒:"); Serial.println(irrigationActive ? "ON" : "OFF");
    }
    
    delay(1000); // 1秒定位周期
}

// 简易编码器（兼容SimpleFOC接口）
Encoder dummyEnc(0, 0, 1);

要点解读

1. 距离触发的分层决策逻辑：平衡安全与作业效率的核心
   距离触发不是单一阈值判断，而是按距离划分“紧急区-预警区-安全区”，匹配不同控制策略，既保障安全，又避免过度避障影响作业效率。
   紧急区（硬安全边界）：如案例5的15cm阈值，触发硬件中断强制停止电机，实现毫秒级响应，应对突发障碍物，避免碰撞损坏设备或伤及作物，是安全底线。
   预警区（主动避障区）：如案例5的40cm阈值，触发模糊逻辑转向，提前调整路径，避免进入紧急区，兼顾避障的主动性与路径的灵活性，适配非结构化农田的不规则障碍物。
   安全区（作业触发区）：距离大于安全阈值时，触发喷洒或沿预设路径巡航，确保机器人在安全范围内高效执行核心任务，避免因过度避障导致作业中断。
   这种分层设计解决了“过度避障导致效率低下、避障不足导致安全隐患”的矛盾，是园林机器人在复杂环境中稳定运行的核心逻辑。

2. 避障与喷洒的协同控制：避免无效作业与资源浪费
   避障与喷洒是园林机器人的两大核心功能，二者必须深度协同，避免“避障时误喷洒”或“喷洒时碰撞”，核心是以避障优先级为核心，实现状态联动。
   优先级控制：避障优先级高于喷洒，案例5、6中，检测到障碍物时，立即暂停喷洒，优先执行避障动作，避障完成后再恢复喷洒，避免喷洒到障碍物（如土块、杂草）造成资源浪费，或喷洒时碰撞导致设备损坏。
   场景化触发：喷洒触发条件与避障状态强绑定，案例4采用“多传感器与逻辑”触发，仅在无避障需求且满足环境条件时喷洒；案例3采用“航点触发”，仅在到达安全航点时喷洒，确保喷洒精准覆盖目标区域，不遗漏、不浪费。
   状态互锁机制：代码中通过布尔变量（如irrigationActive、obstacleDetected）实现状态互锁，避免逻辑冲突，例如避障时irrigationActive强制置为false，防止喷洒与避障同时执行，保障控制逻辑的严谨性。

3. 多传感器融合的感知架构：提升环境适应能力的关键
   单一传感器存在感知盲区（如超声波无法检测透明障碍物，红外易受光照干扰），多传感器融合是解决园林复杂环境感知的核心，三个案例均采用“超声波+红外+其他功能传感器（土壤/光照/GPS）”的融合架构。
   感知互补：案例5中，超声波负责中远距离检测，红外负责近距离补充，避免超声波对透明障碍物（如大棚薄膜）的漏检，同时红外可快速响应近距离障碍物，弥补超声波的响应延迟，形成“远-中-近”全覆盖感知。
   数据校验：通过多传感器数据交叉校验，过滤无效数据，例如案例4、5中对超声波数据设置有效范围（0-150cm），超出范围的数据标记为无效，避免因传感器噪声或干扰导致错误决策。
   功能协同：多传感器不仅服务于避障，还服务于喷洒决策，案例4中，超声波既用于避障，也用于估算植株高度，结合土壤湿度、光照数据，实现“按需喷洒”，避免对空地或无作物区域喷洒，提升资源利用率。

4. 算法与场景的适配性：平衡算力、实时性与效果
   Arduino平台算力有限，算法选择必须兼顾场景需求与硬件约束，三个案例分别采用不同算法，实现“简单场景高效运行、复杂场景灵活应对”。
   基础场景：逻辑判断+简单控制：案例4采用“与逻辑”决策+基础避障，算法简单、算力需求低，适配小型农田、温室等结构化场景，满足定点巡检、按需喷洒的需求，代码易于调试和维护。
   中等复杂场景：模糊逻辑+分层控制：案例5采用模糊逻辑转向，无需精确建模，仅需根据左右距离差输出转向指令，适配垄间、大棚等非规则障碍物场景，算力需求适中，响应速度快，能实现类似人类直觉的避障，避免硬逻辑转向的局限性。
   大范围场景：全局规划+局部重规划：案例6采用多航点全局规划，结合局部重规划，适配大田、果园等大面积场景，通过GPS实现全局路径覆盖，局部避障保障行驶安全，平衡了全局任务的完整性与局部环境的动态适应性，是大面积作业的核心解决方案。

5. 工程落地的可靠性设计：保障系统长期稳定运行
   园林环境复杂（潮湿、粉尘、振动），且机器人依赖电池供电，可靠性设计是工程落地的关键，三个案例均隐含了多项可靠性保障措施，需重点关注。
   传感器可靠性：通过软件滤波过滤传感器噪声，案例4、5对传感器数据进行多次采样或范围校验，避免因环境干扰导致数据跳变；硬件上需对传感器做防水、防尘处理，土壤传感器需密封导线连接处，防止进水短路。
   电源与驱动隔离：BLDC电机启动、避障转向时会产生大电流，易导致Arduino供电不足，案例中电机与主控需采用独立电源，水泵等大功率负载需通过继电器控制，且继电器线圈并联续流二极管，防止反向电动势击穿主控引脚。
   控制逻辑的鲁棒性：设置状态机与异常处理机制，案例2采用状态机（行驶、避障、返回）区分不同工作状态，避免逻辑混乱；案例3中避障完成后自动回归航点，防止因避障导致全局任务中断，同时对无效数据、异常状态设置容错处理，提升系统鲁棒性。
   低功耗与续航优化：对于电池供电的园林机器人，案例4中可引入状态休眠机制，非工作时段让Arduino进入深度休眠，定时唤醒采样，减少电量消耗，延长续航，适配户外长时间作业需求。

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