基于 Arduino 的无刷直流电机（BLDC）四足机器人对角小跑步态控制，是仿生机器人领域中一个经典的动态运动控制课题。它要求四条腿协同工作，以特定的相位差交替腾挪，实现稳定且高效的移动。

1、 主要特点

1. 严格的相位耦合与对角支撑
   对角小跑（Trot）的核心在于其独特的腿部协同模式，这与静态步态（如行走）有本质区别。
   对角线耦合： 机器人的四条腿被分为两组：左前与右后为一组，右前与左后为另一组。这两组腿始终以相反的相位（180°相位差）同步运动。
   双足支撑与腾空： 在一个运动周期内，机器人交替由两组对角线腿支撑身体。在支撑相，对角腿推动身体前进；在摆动相，另一组对角腿抬起前伸。这种模式在特定速度下会形成一个短暂的“腾空相”，即所有腿都离开地面，展现出动态的飞行特性，这正是其高效性的来源。
2. 基于动态平衡的运动机制
   与静态稳定的“爬行”不同，对角小跑是一种典型的动态稳定步态。
   零力矩点（ZMP）控制： 在小跑过程中，机器人的质心（CoM）持续运动，支撑多边形在两个对角点之间快速切换。控制算法必须确保零力矩点始终落在当前的支撑区域内，通过精确控制腿部的落足点和身体的姿态，利用惯性力来抵消倾覆力矩，实现“在运动中保持平衡”。
   能量效率： 相较于行走步态，小跑步态利用了动能和势能的交换，减少了不必要的上下起伏，使得在中高速度下能量利用效率更高。
3. BLDC 电机的高动态响应优势
   动态步态对关节执行器的性能提出了极高要求，BLDC 电机在此展现出巨大优势。
   高功率密度与爆发力： 在支撑相的瞬间，腿部关节需要输出巨大的峰值扭矩以支撑体重并推动身体腾空。BLDC 电机具备高扭矩密度和快速的动态响应能力，能够胜任这种爆发性的工作模式。
   高效散热与持续运行： 动态步态通常意味着长时间的连续运动，BLDC 电机的高效率（>85%）和低发热特性，保证了机器人能够持续稳定运行，不易因过热而降额或停机。

2、应用场景
对角小跑步态凭借其速度与效率的平衡，主要应用于需要快速响应和中距离移动的户外或复杂场景：
野外勘探与搜救机器人： 在地震废墟、山地、丛林等轮式或履带式机器人难以通行的非结构化地形中，具备小跑步态的四足机器人能够快速穿越崎岖路面，执行侦察、物资运送或生命探测任务。
安防巡检与巡逻： 在大型厂区、园区或边境线，机器人可利用小跑步态进行快速例行巡逻，其越障能力和运动速度远超传统轮式机器人，能有效覆盖更大面积。
科研与仿生学研究平台： 作为研究动态平衡、多智能体协同控制、中枢模式发生器（CPG）算法以及生物运动机理的理想载体，帮助科研人员深入理解动物运动并开发更先进的控制策略。
教育与高级竞赛： 在高校机器人社团或高级机器人竞赛中，实现对角小跑是检验学生在机械设计、自动控制、传感器融合和嵌入式编程等多方面综合能力的“试金石”。

3、注意事项
实现稳定的对角小跑步态是一项极具挑战的系统工程，需重点关注以下难点：

1. 高精度关节伺服系统
   执行器要求： 必须使用高性能的 BLDC 关节模组，通常集成了高分辨率编码器（如磁编码器 AS5600）、谐波减速器和力矩闭环驱动器（如 SimpleFOC 兼容方案）。普通航模电调（ESC）无法满足精确的位置和力矩控制需求。
   响应速度： 关节的控制环路延迟必须极低（<1ms），以确保能够快速响应来自上层控制器的指令，实时调整姿态以应对地形变化。
2. 复杂的控制算法与感知融合
   运动规划： 需要设计复杂的足端轨迹规划算法（如椭圆或直线轨迹），并结合逆运动学实时解算出每个关节的角度指令。
   状态估计： 必须融合 IMU（惯性测量单元）数据和关节编码器反馈，通过卡尔曼滤波等算法精确估计机器人的身体姿态（俯仰、横滚角）和质心位置，为平衡控制器提供准确的输入。
3. 机械结构与减震设计
   结构刚度： 机身和腿部连杆必须具备足够的刚度，以承受动态冲击载荷，防止因结构变形导致控制失效。
   被动/主动减震： 在足端或腿部关节加入弹性元件（如弹簧），可以缓冲落地时的冲击力，保护电机和减速器，并提高足底与地面的接触稳定性。
4. 步态平滑切换与稳定性裕度
   步态切换： 从静止或行走步态切换到对角小跑步态，或在不同速度间切换时，必须设计平滑的过渡算法（如基于事件或相位调整），避免因指令突变导致机器人失稳摔倒。
   稳定性判据： 必须实时监控机器人的稳定裕度（如支撑多边形的面积），在检测到即将失稳时（如单腿支撑时间过长），能够自动降速或切换回更稳定的步态。

1、基础方波控制（开环时序）
场景：使用简单的 delay()控制，电机在“抬起”和“放下”状态间瞬时切换。

// 定义电机控制引脚 (假设每个关节一个PWM引脚)
const int LF = 9;  // 左前
const int RF = 10; // 右前
const int LR = 11; // 左后
const int RR = 3;  // 右后

// 步态参数
int stepDuration = 500; // 单步持续时间(ms)
int swingHeight = 200;  // 摆动相高度 (PWM值)
int stanceHeight = 100; // 支撑相高度

void setup() {
  pinMode(LF, OUTPUT);
  pinMode(RF, OUTPUT);
  pinMode(LR, OUTPUT);
  pinMode(RR, OUTPUT);
  Serial.begin(9600);
  
  // 初始状态：所有腿落地
  analogWrite(LF, stanceHeight);
  analogWrite(RF, stanceHeight);
  analogWrite(LR, stanceHeight);
  analogWrite(RR, stanceHeight);
  delay(1000);
}

void loop() {
  // --- 相位1: 左前 & 右后 抬起 (摆动相) ---
  analogWrite(LF, swingHeight);  // 左前抬高
  analogWrite(RR, swingHeight);  // 右后抬高
  analogWrite(RF, stanceHeight); // 右前落地支撑
  analogWrite(LR, stanceHeight); // 左后落地支撑
  
  delay(stepDuration); // 保持此状态一段时间
  
  // --- 相位2: 右前 & 左后 抬起 (摆动相) ---
  analogWrite(RF, swingHeight);  // 右前抬高
  analogWrite(LR, swingHeight);  // 左后抬高
  analogWrite(LF, stanceHeight); // 左前落地支撑
  analogWrite(RR, stanceHeight); // 右后落地支撑
  
  delay(stepDuration);
}

2、带正弦插值的平滑控制
场景：使用 millis()计时和正弦函数计算，使电机角度平滑变化，消除机械冲击。

const int LF = 9, RF = 10, LR = 11, RR = 3;

// 步态参数
unsigned long cycleTime = 2000; // 完整步态周期时间(ms)
unsigned long lastTime = 0;
float phaseOffset = 0.0; // 相位偏移

// 关节角度范围 (模拟)
int minAngle = 100;  // 对应落地
int maxAngle = 200;  // 对应抬腿

void setup() {
  pinMode(LF, OUTPUT);
  pinMode(RF, OUTPUT);
  pinMode(LR, OUTPUT);
  pinMode(RR, OUTPUT);
  Serial.begin(9600);
  lastTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  unsigned long elapsed = currentTime - lastTime;
  
  // 计算当前周期进度 (0.0 到 1.0)
  float t = (float)elapsed / cycleTime;
  if (t >= 1.0) {
    t = 0.0;
    lastTime = currentTime;
  }
  
  // 使用正弦函数生成平滑波形
  // sin(2*PI*t) 范围 [-1, 1]
  // 映射到 [0, 1] 作为抬腿高度系数
  float wave = (sin(2 * PI * t) + 1.0) / 2.0;
  
  // 计算各腿的相位偏移 (对角腿同相)
  float phaseLF = t;           // 左前: 0度
  float phaseRF = t + 0.5;     // 右前: 180度 (相反)
  float phaseLR = t + 0.5;     // 左后: 180度
  float phaseRR = t;           // 右后: 0度
  
  // 确保相位在 [0,1) 范围内
  if (phaseRF >= 1.0) phaseRF -= 1.0;
  if (phaseLR >= 1.0) phaseLR -= 1.0;
  
  // 计算各腿的波形值
  int valLF = map(sin(2 * PI * phaseLF) * 1000, -1000, 1000, minAngle, maxAngle);
  int valRF = map(sin(2 * PI * phaseRF) * 1000, -1000, 1000, minAngle, maxAngle);
  int valLR = map(sin(2 * PI * phaseLR) * 1000, -1000, 1000, minAngle, maxAngle);
  int valRR = map(sin(2 * PI * phaseRR) * 1000, -1000, 1000, minAngle, maxAngle);
  
  // 输出到电机
  analogWrite(LF, valLF);
  analogWrite(RF, valRF);
  analogWrite(LR, valLR);
  analogWrite(RR, valRR);
  
  delay(20); // 控制更新频率
}

3、可调参数与状态机控制
场景：将步态逻辑封装，支持运行时调整速度和步幅，适合作为机器人核心控制库的基础。

// 定义引脚
const int pins[4] = {9, 10, 11, 3}; // LF, RF, LR, RR

// 步态控制器类 (简化版)
class TrotGait {
  public:
    int speed = 1000; // 周期时间，越小越快
    int amplitude = 50; // 摆动幅度
    
    void update() {
      unsigned long now = millis();
      float t = (float)(now % speed) / speed; // 归一化时间
      
      // 定义对角组
      // Group1: 左前(0), 右后(3)
      // Group2: 右前(1), 左后(2)
      
      for (int i=0; i<4; i++) {
        float phase = (i == 0 || i == 3) ? t : t + 0.5; // 对角同相
        if (phase > 1.0) phase -= 1.0;
        
        // 计算正弦波位置
        float pos = sin(2 * PI * phase);
        // 映射到输出范围 (假设中心值150，幅度50)
        int output = 150 + (pos * amplitude);
        
        analogWrite(pins[i], output);
      }
    }
};

TrotGait gait;

void setup() {
  for (int i=0; i<4; i++) {
    pinMode(pins[i], OUTPUT);
  }
  Serial.begin(9600);
  
  // 初始化
  gait.speed = 1500;
  gait.amplitude = 40;
}

void loop() {
  gait.update();
  
  // 模拟接收指令改变速度
  static unsigned long lastCmd = 0;
  if (millis() - lastCmd > 5000) {
    lastCmd = millis();
    gait.speed = random(800, 2000); // 随机改变速度
    Serial.print("Speed changed to: ");
    Serial.println(gait.speed);
  }
  
  delay(30);
}

要点解读
相位同步是核心 (Phase Synchronization)
对角原则：实现“小跑”的关键在于确保左前腿和右后腿始终处于同一相位（同时抬起、同时放下），而另一对角线（右前、左后）则与之相差 180 度（相反）。
实现：代码中通过给不同电机赋予不同的 phaseOffset（如 0.0 和 0.5）来实现。
从“离散”到“连续”的运动控制
案例一问题：使用 delay()和瞬间跳变，会导致电机急启急停，产生巨大电流冲击和机械噪音，容易损坏结构。
最佳实践：如案例二、三所示，使用三角函数（正弦/余弦）​ 或 三次样条插值​ 生成平滑的轨迹曲线。这不仅保护硬件，还能让机器人动作看起来更自然、仿生。
非阻塞时序控制 (Non-blocking Timing)
禁忌：在机器人控制中严禁使用 delay()​ 进行长延时，因为这会阻塞所有其他任务（如传感器读取、通信、平衡计算）。
解决方案：必须基于 millis()或 micros()计算时间差，实现状态机逻辑。这样主循环可以高速运行，同时维持稳定的步态周期。
步态参数化与可调性
关键参数：一个健壮的步态控制器应允许实时调整：
周期 (Cycle Time)：控制走路快慢。
幅度 (Amplitude)：控制抬腿高度（跨越障碍能力）和步幅长度。
占空比 (Duty Cycle)：控制支撑相和摆动相的时间比例（通常各50%）。
案例三展示了如何将这些参数封装，便于调试和优化。
硬件抽象与扩展性
引脚管理：实际机器人可能有 8 个甚至 12 个关节（每个腿3个自由度）。代码应避免硬编码引脚号，而应使用数组或结构体管理。
逆运动学 (IK)：上述代码直接输出 PWM 给电机，假设电机控制的是“抬腿高度”。但在真实机器人中，通常需要逆运动学计算，将“脚末端在空间中的坐标 (X,Y,Z)”转换为各个关节的角度，再转换为 PWM 信号。这是更高级的实现方式。

4、教育级四足机器人对角小跑基础控制
场景定位
面向高校机器人教学、青少年科创项目，核心需求是低成本实现稳定对角小跑、代码逻辑简单易扩展、支持基础改装，让学习者快速理解步态原理，无需复杂编程基础，适配Arduino Uno+基础BLDC驱动的入门配置。
核心步态逻辑
对角小跑的核心是两条对角腿为一组，交替执行“抬起→前进→落地→支撑”的动作，两组交替运动形成持续移动。基础逻辑为：
分组同步：Group1（腿1+腿3）与Group2（腿2+腿4）为两组对角腿，每组同步动作；
时序控制：组1执行动作时，组2执行相反动作，通过时间延时控制步态周期，避免同组腿动作不同步；
参数简化：腿部抬起角度固定、转速固定，通过PWM占空比和方向信号直接控制电机动作，无需复杂位置闭环。

// 教育级四足机器人对角小跑步态控制（Arduino Uno + 4路BLDC驱动）
// 核心：对角腿分组交替运动，实现稳定对角小跑

// ========== 硬件引脚配置 ==========
// 4条腿的控制信号：PWM引脚（控制转速）、方向引脚（控制转向/正反转）
#define LEGS_NUM 4
// 腿1：PWM=D9，方向=D3；腿2：PWM=D10，方向=D4；腿3：PWM=D11，方向=D5；腿4：PWM=D3（复用需调引脚，此处简化为D9-D12，实际按硬件调整）
int legPWMPin[LEGS_NUM] = {9, 10, 11, 6};  // 对应4条腿的PWM输出口
int legDirPin[LEGS_NUM] = {3, 4, 5, 7};     // 对应4条腿的方向控制口

// ========== 步态参数配置 ==========
#define STEP_CYCLE 500    // 完整步态周期（ms）：完成一次对角腿交替的时间
#define LIFT_SPEED 200    // 腿部抬起时的PWM占空比（0-255，值越大转速越快）
#define FORWARD_SPEED 180 // 腿部前进时的PWM占空比
#define LIFT_DURATION 200 // 腿部抬起持续时间（ms）
#define FORWARD_DURATION 300 // 腿部前进持续时间（ms）

// ========== 函数声明 ==========
void setupLegControl();
void setLegState(int legIdx, int pwmVal, bool forwardDir); // 控制单条腿的动作：pwmVal控制转速，forwardDir控制方向
void liftLeg(int legIdx); // 抬起单条腿
void lowerLeg(int legIdx); // 放下单条腿
void forwardLeg(int legIdx); // 腿部前进（电机正转）
void backLeg(int legIdx); // 腿部后退（电机反转）
void diagonalTrot(); // 核心对角小跑控制函数

// ========== 全局变量 ==========
int currentStepCycle = 0; // 当前步态周期计时
bool group1Active = true; // 组1（腿1+腿3）是否为活动组

void setup() {
  setupLegControl(); // 初始化腿部控制引脚
  // 初始状态：所有腿落地，处于支撑状态
  for (int i = 0; i < LEGS_NUM; i++) {
    setLegState(i, 0, true); // 转速0，方向任意（默认支撑）
  }
  Serial.begin(115200);
  Serial.println("教育级四足机器人对角小跑初始化完成");
  Serial.println("程序启动后自动进入对角小跑模式");
}

void loop() {
  // 核心步态控制：调用对角小跑函数
  diagonalTrot();
  // 可通过串口发送指令调整步速（此处预留扩展，默认自动运行）
  if (Serial.available()) {
    char cmd = Serial.read();
    if (cmd == '+') STEP_CYCLE = max(200, STEP_CYCLE - 50); // 加快步速
    if (cmd == '-') STEP_CYCLE = min(1000, STEP_CYCLE + 50); // 减慢步速
    Serial.print("当前步态周期：");
    Serial.print(STEP_CYCLE);
    Serial.println("ms");
  }
}

// ========== 初始化腿部控制引脚 ==========
void setupLegControl() {
  for (int i = 0; i < LEGS_NUM; i++) {
    pinMode(legPWMPin[i], OUTPUT);
    pinMode(legDirPin[i], OUTPUT);
    digitalWrite(legPWMPin[i], 0); // 初始关闭PWM输出
    digitalWrite(legDirPin[i], LOW); // 初始方向设为默认
  }
}

// ========== 控制单条腿的动作（核心函数） ==========
void setLegState(int legIdx, int pwmVal, bool forwardDir) {
  if (legIdx < 0 || legIdx >= LEGS_NUM) return; // 校验腿编号
  analogWrite(legPWMPin[legIdx], pwmVal); // 输出对应转速的PWM
  digitalWrite(legDirPin[legIdx], forwardDir ? HIGH : LOW); // 设置方向（前进/后退）
}

// ========== 抬起单条腿（电机正转，转速为LIFT_SPEED） ==========
void liftLeg(int legIdx) {
  setLegState(legIdx, LIFT_SPEED, true);
}

// ========== 放下单条腿（电机停止，PWM为0） ==========
void lowerLeg(int legIdx) {
  setLegState(legIdx, 0, true); // 方向不影响，转速为0即停止
}

// ========== 腿部前进（电机正转，转速为FORWARD_SPEED） ==========
void forwardLeg(int legIdx) {
  setLegState(legIdx, FORWARD_SPEED, true);
}

// ========== 腿部后退（电机反转，转速为FORWARD_SPEED） ==========
void backLeg(int legIdx) {
  setLegState(legIdx, FORWARD_SPEED, false);
}

// ========== 核心对角小跑控制函数 ==========
void diagonalTrot() {
  static unsigned long lastTime = 0;
  unsigned long currentTime = millis();

  // 按步态周期分两个阶段控制：前半周期组1动作，后半周期组2动作
  if (currentTime - lastTime < STEP_CYCLE / 2) {
    // 阶段1：组1（腿1+腿3）抬起前进，组2（腿2+腿4）落地支撑
    if (group1Active) {
      liftLeg(0); // 腿1抬起
      forwardLeg(0); // 腿1前进
      liftLeg(2); // 腿3抬起
      forwardLeg(2); // 腿3前进
      lowerLeg(1); // 腿2落地支撑
      lowerLeg(3); // 腿4落地支撑
    } else {
      liftLeg(1); // 腿2抬起
      forwardLeg(1); // 腿2前进
      liftLeg(3); // 腿4抬起
      forwardLeg(3); // 腿4前进
      lowerLeg(0); // 腿1落地支撑
      lowerLeg(2); // 腿3落地支撑
    }
  } else {
    // 阶段2：交换组动作，实现交替
    group1Active = !group1Active;
    lastTime = currentTime; // 重置计时，开始下一个步态周期
  }
}

5、工业巡检四足机器人速度可调对角小跑
场景定位
适用于工业巡检场景（如变电站、工厂车间），核心需求是对角小跑速度可调、支持远程控制、具备基础安全保护，需适配更高性能的Arduino硬件和更稳定的BLDC控制，支持不同巡检速度切换，满足不同工况的巡检需求。
核心步态逻辑
在基础对角小跑的基础上，新增速度调节与远程控制，核心逻辑升级为：
速度映射：将速度指令（0-255）映射为PWM占空比和步态周期，速度越高，PWM占空比越大，步态周期越短；
时序自适应：根据速度动态调整腿部抬起/前进的持续时间，高速时缩短动作时间，避免动作冲突；
远程闭环：通过串口接收远程速度指令，实时更新步态参数，同时通过编码器反馈电机位置，确保腿部动作一致性。

// 工业巡检四足机器人速度可调对角小跑（Arduino Mega 2560 + 闭环BLDC驱动 + ESP32远程控制）
// 核心：远程速度指令→步态参数映射，实现速度可调的对角小跑

// ========== 硬件引脚配置 ==========
#define LEGS_NUM 4
int legPWMPin[LEGS_NUM] = {2, 3, 4, 5};  // 4条腿的PWM输出口
int legDirPin[LEGS_NUM] = {6, 7, 8, 9};  // 4条腿的方向控制口
int legEnablePin[LEGS_NUM] = {10, 11, 12, 13}; // 4条腿的使能控制口（高电平使能）

// ========== 步态参数配置（可远程调整） ==========
int currentSpeed = 100; // 当前速度指令（0-255，0停止，255最快）
#define MAX_SPEED 255
#define MIN_SPEED 0
#define BASE_CYCLE 800  // 基础步态周期（ms，速度0时的周期）
#define MAX_CYCLE 2000  // 最大步态周期（ms，速度最慢时的周期）
#define MIN_CYCLE 400   // 最小步态周期（ms，速度最快时的周期）

// ========== 函数声明 ==========
void setupLegControl();
void setLegSpeed(int legIdx, int speed);
void liftLegWithSpeed(int legIdx, int speed);
void forwardLegWithSpeed(int legIdx, int speed);
void lowerLeg(int legIdx);
void adjustCycleBySpeed(); // 根据速度调整步态周期
void serialParseSpeedCommand(); // 解析串口速度指令

// ========== 全局变量 ==========
unsigned long lastStepTime = 0;
bool group1Active = true;
int stepCycle; // 当前步态周期，根据速度动态计算

void setup() {
  setupLegControl(); // 初始化腿部控制
  // 初始化使能信号，所有腿使能
  for (int i = 0; i < LEGS_NUM; i++) {
    digitalWrite(legEnablePin[i], HIGH);
  }
  Serial.begin(115200); // 与ESP32通信的串口
  Serial.println("工业巡检四足机器人对角小跑初始化完成");
  Serial.println("等待远程速度指令（格式：S+数值，如S150）");
}

void loop() {
  1. 解析串口速度指令，更新当前速度
  serialParseSpeedCommand();

  2. 根据速度调整步态周期
  adjustCycleBySpeed();

  3. 执行对角小跑动作
  unsigned long currentTime = millis();
  if (currentTime - lastStepTime < stepCycle / 2) {
    if (group1Active) {
      liftLegWithSpeed(0, currentSpeed);
      forwardLegWithSpeed(0, currentSpeed);
      liftLegWithSpeed(2, currentSpeed);
      forwardLegWithSpeed(2, currentSpeed);
      lowerLeg(1);
      lowerLeg(3);
    } else {
      liftLegWithSpeed(1, currentSpeed);
      forwardLegWithSpeed(1, currentSpeed);
      liftLegWithSpeed(3, currentSpeed);
      forwardLegWithSpeed(3, currentSpeed);
      lowerLeg(0);
      lowerLeg(2);
    }
  } else {
    group1Active = !group1Active;
    lastStepTime = currentTime;
  }

  4. 检查避障信号（预留扩展，此处简化）
  // 若避障传感器触发，可暂停步态，此处仅留接口
}

// ========== 初始化腿部控制引脚 ==========
void setupLegControl() {
  for (int i = 0; i < LEGS_NUM; i++) {
    pinMode(legPWMPin[i], OUTPUT);
    pinMode(legDirPin[i], OUTPUT);
    pinMode(legEnablePin[i], OUTPUT);
    analogWrite(legPWMPin[i], 0);
    digitalWrite(legDirPin[i], LOW);
  }
}

// ========== 设置单条腿的转速（PWM占空比与速度指令线性映射） ==========
void setLegSpeed(int legIdx, int speed) {
  if (legIdx < 0 || legIdx >= LEGS_NUM || speed < MIN_SPEED || speed > MAX_SPEED) return;
  // 速度0-255映射到PWM 0-255，可根据实际情况调整（如加死区防止堵转）
  int pwmVal = speed;
  analogWrite(legPWMPin[legIdx], pwmVal);
}

// ========== 抬起单条腿（带速度控制） ==========
void liftLegWithSpeed(int legIdx, int speed) {
  setLegSpeed(legIdx, speed);
  digitalWrite(legDirPin[legIdx], HIGH); // 正转抬起
}

// ========== 腿部前进（带速度控制） ==========
void forwardLegWithSpeed(int legIdx, int speed) {
  setLegSpeed(legIdx, speed);
  digitalWrite(legDirPin[legIdx], HIGH); // 正转前进（与抬起方向一致，根据机械结构调整）
}

// ========== 放下单条腿 ==========
void lowerLeg(int legIdx) {
  setLegSpeed(legIdx, 0);
  digitalWrite(legDirPin[legIdx], LOW); // 停止时方向无关紧要，设为默认低电平
}

// ========== 根据速度动态调整步态周期（核心自适应逻辑） ==========
void adjustCycleBySpeed() {
  // 速度与步态周期的线性映射：速度越高，周期越短
  // 公式：stepCycle = BASE_CYCLE - (currentSpeed / MAX_SPEED) * (BASE_CYCLE - MIN_CYCLE)
  stepCycle = map(currentSpeed, MIN_SPEED, MAX_SPEED, MAX_CYCLE, MIN_CYCLE);
  // 限制周期范围，避免超出硬件能力
  stepCycle = constrain(stepCycle, MIN_CYCLE, MAX_CYCLE);
}

// ========== 解析串口速度指令（如S150，设置速度为150） ==========
void serialParseSpeedCommand() {
  if (Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    if (cmd.length() >= 2 && cmd[0] == 'S') {
      int newSpeed = cmd.substring(1).toInt();
      if (newSpeed >= MIN_SPEED && newSpeed <= MAX_SPEED) {
        currentSpeed = newSpeed;
        Serial.print("已设置速度：");
        Serial.println(currentSpeed);
      } else {
        Serial.println("错误：速度指令需在0-255之间");
      }
    } else {
      Serial.println("错误：指令格式为S+数值，如S150");
    }
  }
}

6、避障自适应对角小跑控制
场景定位
适用于户外复杂环境巡检、应急救援场景，核心需求是对角小跑过程中实时避障、步态参数自适应调整、应对轻度地形变化，需引入传感器感知环境，动态调整步态，确保机器人在障碍物前减速、避障后恢复正常步态，提升环境适应能力。
核心步态逻辑
在基础对角小跑的基础上，新增环境感知与步态自适应，核心逻辑升级为：
障碍物感知：通过超声波传感器检测前方、左侧、右侧的障碍物距离，设定安全阈值；
步态自适应：障碍物距离小于安全阈值时，减速并调整前进方向（如左避障则右腿动作变化，实现左转）；
姿态补偿：通过IMU检测机身倾斜，动态调整对角腿的抬起高度，防止侧翻；
安全联锁：避障触发时暂停前进，调整方向后恢复步态，确保避障过程中稳定。

// 避障自适应对角小跑控制（Arduino Mega 2560 + 多超声波避障 + IMU姿态补偿）
// 核心：传感器感知环境→动态调整步速与方向，实现自适应避障的对角小跑

// ========== 硬件引脚配置 ==========
#define LEGS_NUM 4
int legPWMPin[LEGS_NUM] = {2, 3, 4, 5};
int legDirPin[LEGS_NUM] = {6, 7, 8, 9};
int legEnablePin[LEGS_NUM] = {10, 11, 12, 13};

// 避障传感器引脚：前（14）、左（15）、右（16）、后（17）
#define FORWARD_SENSOR 14
#define LEFT_SENSOR 15
#define RIGHT_SENSOR 16
#define BACK_SENSOR 17

// ========== 步态与避障参数配置 ==========
int baseSpeed = 150;    // 基础前进速度
int obstacleThreshold = 200; // 障碍物安全距离阈值（cm，超声波测量值）
int currentSpeed = baseSpeed;
int turnAdjust = 0; // 转向调整量：正为右转，负为左转

// ========== 函数声明 ==========
void setupLegControl();
int getUltrasonicDistance(int sensorPin); // 读取超声波距离
void checkObstacle(); // 检查障碍物并调整速度与方向
void adjustGaitForObstacle(); // 根据障碍物调整步态
void normalDiagonalTrot(); // 正常无障碍物时的对角小跑
void compensateAttitude(); // 姿态补偿（预留接口）

// ========== 全局变量 ==========
unsigned long lastStepTime = 0;
bool group1Active = true;
int stepCycle = 600;

void setup() {
  setupLegControl();
  // 初始化避障传感器引脚
  pinMode(FORWARD_SENSOR, INPUT);
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);
  pinMode(BACK_SENSOR, INPUT);
  // 初始化IMU（此处简化，实际需调用MPU6050库）
  Serial.begin(115200);
  Serial.println("避障自适应对角小跑系统初始化完成");
}

void loop() {
  1. 检测障碍物，更新速度与转向参数
  checkObstacle();

  2. 姿态补偿（预留扩展，此处简化调用）
  compensateAttitude();

  3. 根据障碍物状态执行步态
  if (turnAdjust != 0) {
    adjustGaitForObstacle(); // 有障碍物，执行避障步态
  } else {
    normalDiagonalTrot(); // 无障碍物，执行正常对角小跑
  }

  4. 串口上报状态
  Serial.print("当前速度：");
  Serial.print(currentSpeed);
  Serial.print(" 转向调整：");
  Serial.print(turnAdjust);
  Serial.print(" 前方距离：");
  Serial.println(getUltrasonicDistance(FORWARD_SENSOR));
}

// ========== 初始化腿部控制 ==========
void setupLegControl() {
  for (int i = 0; i < LEGS_NUM; i++) {
    pinMode(legPWMPin[i], OUTPUT);
    pinMode(legDirPin[i], OUTPUT);
    pinMode(legEnablePin[i], OUTPUT);
    digitalWrite(legEnablePin[i], HIGH);
    analogWrite(legPWMPin[i], 0);
    digitalWrite(legDirPin[i], LOW);
  }
}

// ========== 读取超声波传感器距离（简化实现，实际需驱动传感器） ==========
int getUltrasonicDistance(int sensorPin) {
  // 此处为简化代码，实际需编写超声波传感器驱动（如HC-SR04的Trig/Echo逻辑）
  // 假设传感器返回距离值（0-1000cm），此处模拟读取
  // 实际代码需根据传感器硬件补充：发送触发信号、接收回波、计算距离
  int distance = analogRead(sensorPin) / 10; // 模拟转换，实际需适配
  return constrain(distance, 0, 1000);
}

// ========== 检查障碍物并调整速度与方向 ==========
void checkObstacle() {
  int forwardDist = getUltrasonicDistance(FORWARD_SENSOR);
  int leftDist = getUltrasonicDistance(LEFT_SENSOR);
  int rightDist = getUltrasonicDistance(RIGHT_SENSOR);

  turnAdjust = 0; // 默认不转向

  1. 前方有障碍物，减速并准备转向
  if (forwardDist < obstacleThreshold) {
    currentSpeed = baseSpeed / 2; // 减速至基础速度的50%
    if (leftDist > rightDist) {
      turnAdjust = -30; // 左侧距离更远，左转避障
    } else if (rightDist > leftDist) {
      turnAdjust = 30; // 右侧距离更远，右转避障
    } else {
      turnAdjust = 0; // 两侧距离相近，停止避障（此处可扩展为后退）
    }
  }
  2. 左右有障碍物，调整转向
  else if (leftDist < obstacleThreshold) {
    turnAdjust = 30; // 左侧有障碍，右转
  } else if (rightDist < obstacleThreshold) {
    turnAdjust = -30; // 右侧有障碍，左转
  }
  3. 无障碍物，恢复正常速度
  else {
    currentSpeed = baseSpeed;
    turnAdjust = 0;
  }
}

// ========== 避障时的自适应步态（调整对角腿转速实现转向） ==========
void adjustGaitForObstacle() {
  unsigned long currentTime = millis();
  if (currentTime - lastStepTime < stepCycle / 2) {
    if (group1Active) {
      // 转向调整：通过左右腿速度差实现转向，左转则右腿加速，右转则左腿加速
      liftLegWithSpeed(0, currentSpeed + turnAdjust);
      forwardLegWithSpeed(0, currentSpeed + turnAdjust);
      liftLegWithSpeed(2, currentSpeed - turnAdjust);
      forwardLegWithSpeed(2, currentSpeed - turnAdjust);
      lowerLeg(1);
      lowerLeg(3);
    } else {
      liftLegWithSpeed(1, currentSpeed - turnAdjust);
      forwardLegWithSpeed(1, currentSpeed - turnAdjust);
      liftLegWithSpeed(3, currentSpeed + turnAdjust);
      forwardLegWithSpeed(3, currentSpeed + turnAdjust);
      lowerLeg(0);
      lowerLeg(2);
    }
  } else {
    group1Active = !group1Active;
    lastStepTime = currentTime;
  }
}

// ========== 正常无障碍物时的对角小跑 ==========
void normalDiagonalTrot() {
  unsigned long currentTime = millis();
  if (currentTime - lastStepTime < stepCycle / 2) {
    if (group1Active) {
      liftLegWithSpeed(0, currentSpeed);
      forwardLegWithSpeed(0, currentSpeed);
      liftLegWithSpeed(2, currentSpeed);
      forwardLegWithSpeed(2, currentSpeed);
      lowerLeg(1);
      lowerLeg(3);
    } else {
      liftLegWithSpeed(1, currentSpeed);
      forwardLegWithSpeed(1, currentSpeed);
      liftLegWithSpeed(3, currentSpeed);
      forwardLegWithSpeed(3, currentSpeed);
      lowerLeg(0);
      lowerLeg(2);
    }
  } else {
    group1Active = !group1Active;
    lastStepTime = currentTime;
  }
}

// ========== 姿态补偿（预留接口，可根据IMU姿态数据调整腿部抬起高度） ==========
void compensateAttitude() {
  // 此处需集成MPU6050库，读取机身倾斜角度（roll/pitch）
  // 若机身向左倾斜，可适当提高右侧腿的抬起高度，防止侧翻
  // 示例逻辑（简化）：
  // int roll = getRollAngle(); // 获取机身横滚角
  // if (roll > 5) {
  //   baseLiftHeight += roll / 2; // 左侧倾斜，右侧腿增加抬起高度
  // } else if (roll < -5) {
  //   baseLiftHeight -= roll / 2;
  // }
}

// ========== 辅助函数：带速度控制的腿部动作 ==========
void liftLegWithSpeed(int legIdx, int speed) {
  if (legIdx < 0 || legIdx >= LEGS_NUM) return;
  analogWrite(legPWMPin[legIdx], speed);
  digitalWrite(legDirPin[legIdx], HIGH);
}

void forwardLegWithSpeed(int legIdx, int speed) {
  if (legIdx < 0 || legIdx >= LEGS_NUM) return;
  analogWrite(legPWMPin[legIdx], speed);
  digitalWrite(legDirPin[legIdx], HIGH);
}

void lowerLeg(int legIdx) {
  if (legIdx < 0 || legIdx >= LEGS_NUM) return;
  analogWrite(legPWMPin[legIdx], 0);
  digitalWrite(legDirPin[legIdx], LOW);
}

要点解读

1. 步态时序同步：对角交替的“时间核心”，决定步态稳定性
   对角小跑的本质是两组对角腿按固定时序交替动作，时序的精准控制直接决定机器人是否摔倒，是步态稳定的核心：
   时序划分原则：将完整步态周期分为两个等时长阶段，前半周期一组对角腿抬起前进，后半周期切换为另一组，确保任意时刻有两条对角腿支撑机身，维持平衡；
   同步性保障：同组两条腿的动作必须完全同步（同时抬起、同时前进、同时落地），否则会导致机身倾斜；代码中需通过同一函数调用、同一参数控制同组腿，避免动作偏差；
   周期适配：步态周期需适配电机响应速度，过短会导致电机无法完成动作，过长则效率低下，教育级机器人建议500-800ms，工业级可缩短至400-600ms，需根据电机扭矩和减速比调整。
2. 电机控制适配：BLDC与步态的“动力匹配”，保障动作精准
   BLDC电机是步态执行的动力核心，其控制方式需与步态需求深度适配，否则会出现动作滞后、转速不足等问题：
   控制模式选择：基础步态采用开环PWM控制（成本低、响应快），适用于教育级场景；工业级需采用速度闭环（编码器反馈）或位置闭环（PID控制），确保腿部动作的重复性和精准度，避免因电压波动导致转速偏差；
   参数匹配：PWM占空比与电机转速的映射需提前标定，不同扭矩的电机、不同减速比的减速器，占空比与实际转速的关系不同，需通过实验测试标定，避免参数设置不当导致电机堵转或转速不足；
   方向与使能控制：腿部抬起与落地需切换电机正反转或使能/失能，切换前需先降为0转速，防止带载切换产生大电流冲击驱动板，代码中需加入“降速→切换→升速”的逻辑，保护硬件。
3. 环境感知融合：避障与步态的“闭环协同”，提升环境适应性
   户外复杂场景需引入环境感知，让步态具备自适应能力，环境感知与步态控制的融合是关键：
   传感器选型逻辑：避障用超声波（成本低、覆盖广）或激光雷达（精度高），姿态检测用IMU（检测倾斜），地形检测用压力传感器（检测地面硬度），根据场景需求组合传感器，避免冗余；
   感知-决策-执行闭环：传感器数据→障碍物/姿态判断→步态参数调整（速度、方向、腿部高度）→电机执行，形成闭环控制，确保机器人能实时应对环境变化，代码中需预留传感器接口，方便扩展；
   优先级控制：避障优先级高于正常步态，姿态补偿优先级高于速度调整，确保在障碍物前优先减速转向，机身倾斜时优先调整腿部高度，避免因优先级混乱导致失控。
4. 机械结构适配：步态与硬件的“物理匹配”，避免机械损耗
   步态控制需适配机械结构设计，否则会出现机械磨损、动作卡顿，甚至损坏结构：
   腿部自由度匹配：对角小跑需要腿部具备至少2个自由度（抬腿、前后摆动），若机械结构仅1个自由度，需通过电机正反转模拟抬腿动作，代码中需根据机械结构调整电机动作逻辑；
   负载与扭矩匹配：机器人自重+负载的重量，需由4条腿分担，单腿电机扭矩需满足抬腿和支撑的需求，扭矩不足会导致抬腿失败，需通过机械计算选型电机，代码中需设置转速上限，防止过载；
   关节间隙补偿：机械关节存在间隙，会导致动作滞后，代码中可通过增加预动作补偿（如提前输出PWM让电机克服间隙），或通过PID闭环控制消除间隙影响，避免动作卡顿。
5. 安全保护与调试：工程落地的“底线保障”，降低维护成本
   四足机器人对角小跑涉及高速运动和机械结构，安全保护和高效调试是工程落地的关键：
   硬件保护：加入过流保护（保险丝）、过压保护（稳压模块）、短路保护（驱动板的短路保护功能），防止电机堵转、电源反接导致硬件烧毁；代码中设置最大PWM占空比，限制电机转速，避免超速失控；
   软件保护：无传感器反馈时，设置速度下限和上限，避免电机堵转；障碍物距离过近时触发急停（所有电机停止），防止碰撞；传感器失效时切换到基础步态或停止，确保安全；
   调试工具与方法：通过串口实时打印电机转速、步态周期、传感器数据，便于定位问题；分阶段调试（先单腿动作→再同组同步→最后对角交替），逐步验证功能；通过PWM占空比调节，匹配机械结构的响应速度，避免调试时盲目修改参数导致硬件损坏。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。