场景实战：基于STM32的六足机器人运动控制与姿态调节解决方案解析

文章目录

• • 一、方案整体设计与原理说明
  • • 1.1 核心原理
    • 1.2 整体架构流程图
  • 二、硬件选型与接线
  • • 2.1 核心硬件清单
    • 2.2 详细接线说明
    • • （1）STM32与PCA9685舵机驱动板接线（I2C通信）
      • （2）PCA9685与舵机接线
      • （3）STM32与MPU6050接线（I2C通信）
      • （4）STM32与NRF24L01接线（SPI通信）
      • （5）电源接线
  • 三、软件开发环境搭建
  • • 3.1 环境准备
    • 3.2 STM32CubeMX初始化配置步骤
    • • 步骤1：新建工程
      • 步骤2：配置RCC时钟
      • 步骤3：配置I2C外设（MPU6050+PCA9685）
      • 步骤4：配置SPI外设（NRF24L01）
      • 步骤5：配置GPIO引脚（NRF24L01控制）
      • 步骤6：生成工程代码
  • 四、核心代码编写
  • • 4.1 头文件与全局变量定义（main.h）
    • 4.2 外设初始化代码（main.c）
    • 4.3 PCA9685舵机驱动函数（main.c续）
    • 4.4 MPU6050姿态检测函数（main.c续）
    • 4.5 PID控制与姿态调节函数（main.c续）
    • 4.6 六足步态规划函数（main.c续）
    • 4.7 NRF24L01无线通信函数（main.c续）
    • 4.8 主函数（main.c续）
  • 五、代码烧录与调试
  • • 5.1 代码编译与烧录
    • 5.2 硬件调试步骤
    • • 步骤1：基础功能验证
      • 步骤2：姿态检测调试
      • 步骤3：无线通信调试
      • 步骤4：步态运动调试
    • 5.3 常见问题与解决
  • 六、功能扩展建议
  • 总结

一、方案整体设计与原理说明

需要实现的是基于STM32的六足机器人运动控制与姿态调节系统，核心功能包含六足标准步态规划（三角步态）、基于MPU6050的姿态检测与实时调节、NRF24L01无线遥控、18路舵机精准驱动四大模块。本方案以STM32F103ZET6为核心控制器，通过预规划的步态表控制18个舵机（每足3个：髋关节、膝关节、踝关节）实现前进/后退/左转/右转/原地旋转等运动；利用MPU6050采集陀螺仪+加速度计数据，通过DMP算法解算姿态角（俯仰/横滚），结合PID算法实时调节舵机角度补偿机器人倾斜；通过NRF24L01接收遥控器指令，实现无线运动控制。整个方案模块化拆解，代码注释详尽，零基础小白可完全复刻落地。

1.1 核心原理

• 六足步态规划原理：六足机器人采用经典三角步态（Tripod Gait），将6条腿分为两组（左前/右中/左后为A组，右前/左中/右后为B组），两组交替抬升/落地：A组抬升前进时B组落地支撑，B组抬升前进时A组落地支撑，通过预定义的舵机角度表控制每一步的关节角度，实现平稳运动。
• 舵机控制原理：STM32定时器输出50Hz PWM波（20ms周期），通过调节高电平占空比（0.5ms-2.5ms对应舵机0°-180°）精准控制舵机角度；采用定时器通道分时输出+锁存方式，驱动18路舵机且无信号干扰。
• 姿态检测与调节原理：MPU6050通过I2C与STM32通信，内置DMP算法可直接输出四元数，转换为俯仰角（Pitch）和横滚角（Roll）；当机器人倾斜时，PID算法根据姿态偏差计算舵机补偿角度，调整腿部高度使机器人保持水平。
• 无线遥控原理：NRF24L01通过SPI与STM32通信，接收遥控器（另一个NRF24L01+按键）发送的运动指令（前进/后退/左转/右转/停止），STM32解析指令后切换对应步态表，实现无线控制。

1.2 整体架构流程图

以下是六足机器人系统完整工作流程的Mermaid流程图

#mermaid-svg-igF9abgp1bIQWzyV{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-igF9abgp1bIQWzyV .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-igF9abgp1bIQWzyV .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-igF9abgp1bIQWzyV .error-icon{fill:#552222;}#mermaid-svg-igF9abgp1bIQWzyV .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-igF9abgp1bIQWzyV .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-igF9abgp1bIQWzyV .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-igF9abgp1bIQWzyV .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-igF9abgp1bIQWzyV .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-igF9abgp1bIQWzyV .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-igF9abgp1bIQWzyV .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-igF9abgp1bIQWzyV .marker{fill:#333333;stroke:#333333;}#mermaid-svg-igF9abgp1bIQWzyV .marker.cross{stroke:#333333;}#mermaid-svg-igF9abgp1bIQWzyV svg{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-igF9abgp1bIQWzyV p{margin:0;}#mermaid-svg-igF9abgp1bIQWzyV .label{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;color:#333;}#mermaid-svg-igF9abgp1bIQWzyV .cluster-label text{fill:#333;}#mermaid-svg-igF9abgp1bIQWzyV .cluster-label span{color:#333;}#mermaid-svg-igF9abgp1bIQWzyV .cluster-label span p{background-color:transparent;}#mermaid-svg-igF9abgp1bIQWzyV .label text,#mermaid-svg-igF9abgp1bIQWzyV span{fill:#333;color:#333;}#mermaid-svg-igF9abgp1bIQWzyV .node rect,#mermaid-svg-igF9abgp1bIQWzyV .node circle,#mermaid-svg-igF9abgp1bIQWzyV .node ellipse,#mermaid-svg-igF9abgp1bIQWzyV .node polygon,#mermaid-svg-igF9abgp1bIQWzyV .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-igF9abgp1bIQWzyV .rough-node .label text,#mermaid-svg-igF9abgp1bIQWzyV .node .label text,#mermaid-svg-igF9abgp1bIQWzyV .image-shape .label,#mermaid-svg-igF9abgp1bIQWzyV .icon-shape .label{text-anchor:middle;}#mermaid-svg-igF9abgp1bIQWzyV .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-igF9abgp1bIQWzyV .rough-node .label,#mermaid-svg-igF9abgp1bIQWzyV .node .label,#mermaid-svg-igF9abgp1bIQWzyV .image-shape .label,#mermaid-svg-igF9abgp1bIQWzyV .icon-shape .label{text-align:center;}#mermaid-svg-igF9abgp1bIQWzyV .node.clickable{cursor:pointer;}#mermaid-svg-igF9abgp1bIQWzyV .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-igF9abgp1bIQWzyV .arrowheadPath{fill:#333333;}#mermaid-svg-igF9abgp1bIQWzyV .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-igF9abgp1bIQWzyV .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-igF9abgp1bIQWzyV .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-igF9abgp1bIQWzyV .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-igF9abgp1bIQWzyV .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-igF9abgp1bIQWzyV .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-igF9abgp1bIQWzyV .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-igF9abgp1bIQWzyV .cluster text{fill:#333;}#mermaid-svg-igF9abgp1bIQWzyV .cluster span{color:#333;}#mermaid-svg-igF9abgp1bIQWzyV div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-igF9abgp1bIQWzyV .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-igF9abgp1bIQWzyV rect.text{fill:none;stroke-width:0;}#mermaid-svg-igF9abgp1bIQWzyV .icon-shape,#mermaid-svg-igF9abgp1bIQWzyV .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-igF9abgp1bIQWzyV .icon-shape p,#mermaid-svg-igF9abgp1bIQWzyV .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-igF9abgp1bIQWzyV .icon-shape rect,#mermaid-svg-igF9abgp1bIQWzyV .image-shape rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-igF9abgp1bIQWzyV .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-igF9abgp1bIQWzyV .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-igF9abgp1bIQWzyV :root{–mermaid-font-family:\”trebuchet ms\”,verdana,arial,sans-serif;}#mermaid-svg-igF9abgp1bIQWzyV .darkStyle>*{fill:#2c3e50!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .darkStyle span{fill:#2c3e50!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .darkStyle tspan{fill:#ffffff!important;}#mermaid-svg-igF9abgp1bIQWzyV .startEnd>*{fill:#e74c3c!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .startEnd span{fill:#e74c3c!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .startEnd tspan{fill:#ffffff!important;}#mermaid-svg-igF9abgp1bIQWzyV .decision>*{fill:#3498db!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .decision span{fill:#3498db!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .decision tspan{fill:#ffffff!important;}#mermaid-svg-igF9abgp1bIQWzyV .process>*{fill:#27ae60!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .process span{fill:#27ae60!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .process tspan{fill:#ffffff!important;}#mermaid-svg-igF9abgp1bIQWzyV .sensor>*{fill:#9b59b6!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .sensor span{fill:#9b59b6!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .sensor tspan{fill:#ffffff!important;}#mermaid-svg-igF9abgp1bIQWzyV .motor>*{fill:#f39c12!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .motor span{fill:#f39c12!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .motor tspan{fill:#ffffff!important;}#mermaid-svg-igF9abgp1bIQWzyV .wireless>*{fill:#1abc9c!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .wireless span{fill:#1abc9c!important;stroke:#ffffff!important;stroke-width:2px!important;color:#ffffff!important;fontSize:14px!important;fontFamily:Arial!important;}#mermaid-svg-igF9abgp1bIQWzyV .wireless tspan{fill:#ffffff!important;}

是

否

是

否

系统上电初始化

RCC/I2C/SPI/TIM/PWM配置

初始化舵机为中立位

初始化MPU6050并校准

初始化NRF24L01接收模式

接收无线指令?

解析指令：前进/后退/左转/右转

保持当前步态/停止

加载对应步态角度表

MPU6050采集姿态数据（Pitch/Roll）

解算姿态偏差（与水平基准对比）

PID算法计算舵机补偿角度

融合步态角度+补偿角度

输出PWM控制18路舵机动作

步态执行完成?

切换步态下一帧角度

二、硬件选型与接线

2.1 核心硬件清单

2.2 详细接线说明

（1）STM32与PCA9685舵机驱动板接线（I2C通信）

PCA9685支持I2C总线扩展，可级联2块实现18路舵机驱动，接线如下：

（2）PCA9685与舵机接线

每块PCA9685有16路PWM输出，18路舵机分配如下：

• 舵机接线：棕色=GND，红色=5V，橙色=PWM（接PCA9685通道）；
• 供电：PCA9685的V+引脚接11.1V锂电池（舵机直接供电），VCC接5V（逻辑供电）。

（3）STM32与MPU6050接线（I2C通信）

（4）STM32与NRF24L01接线（SPI通信）

（5）电源接线

• 主电源：11.1V锂电池 → 舵机驱动板V+（给18路舵机供电）；
• 辅助电源：11.1V锂电池 → DC-DC 5V模块 → STM32 5V引脚、PCA9685 VCC、MPU6050 VCC；
• 3.3V电源：STM32 3.3V引脚 → NRF24L01 VCC。

三、软件开发环境搭建

3.1 环境准备

3.2 STM32CubeMX初始化配置步骤

步骤1：新建工程

• 打开STM32CubeMX，点击“New Project”，在搜索框输入“STM32F103ZET6”，选择对应型号（STM32F103ZETx）后点击“Start Project”。
• 弹出“Project Manager”提示框，点击“OK”跳过（默认配置即可）。

步骤2：配置RCC时钟

• 左侧菜单栏选择“RCC”：
  • High Speed Clock (HSE)：选择“Crystal/Ceramic Resonator”（外部8MHz晶振），启用HSE；
  • Low Speed Clock (LSI)：保持默认（内部低速晶振）；
• 点击顶部“Clock Configuration”，配置系统时钟为72MHz：
  • HSE = 8MHz → PLLMUL = 9 → SYSCLK = 72MHz；
  • AHB Prescaler = 1（HCLK=72MHz）；
  • APB1 Prescaler = 2（PCLK1=36MHz）；
  • APB2 Prescaler = 1（PCLK2=72MHz）；
  • I2C1/2时钟源：PCLK1（36MHz）；
  • SPI1时钟源：PCLK2（72MHz）。

步骤3：配置I2C外设（MPU6050+PCA9685）

• 左侧菜单栏选择“I2C1”：
  • 模式选择“I2C”（标准模式）；
  • 配置参数：Clock Speed = 100kHz，Addressing Mode = 7-bit；
  • 引脚映射：PB6（SCL）、PB7（SDA）；
• 左侧菜单栏选择“I2C2”：
  • 模式选择“I2C”（标准模式）；
  • 配置参数：Clock Speed = 100kHz，Addressing Mode = 7-bit；
  • 引脚映射：PB10（SCL）、PB11（SDA）。

步骤4：配置SPI外设（NRF24L01）

• 左侧菜单栏选择“SPI1”：
  • 模式选择“Full-Duplex Master”（全双工主机模式）；
  • 配置参数：
    • Clock Prescaler = 8（72MHz/8=9MHz，匹配NRF24L01）；
    • CPOL = Low，CPHA = 1Edge；
    • Data Size = 8 Bits；
    • NSS = Software；
  • 引脚映射：PA5（SCK）、PA6（MISO）、PA7（MOSI）。

步骤5：配置GPIO引脚（NRF24L01控制）

• 左侧菜单栏选择“GPIO”：
  • PB0：设置为“Output Push Pull”，命名为“NRF_CE”，初始电平“Low”；
  • PB1：设置为“Output Push Pull”，命名为“NRF_CSN”，初始电平“High”；
  • 所有GPIO速度设为“High”。

步骤6：生成工程代码

• 点击顶部“Project Manager”：
  • Project Name：设置为“Hexapod_Robot”；
  • Project Path：选择非中文路径（如D:\\STM32_Projects\\Hexapod_Robot）；
  • Toolchain/IDE：选择“MDK-ARM”，版本“V5”；
• 点击“Code Generator”：
  • 勾选“Generate peripheral initialization as a pair of ‘.c/.h’ files per peripheral”；
  • 勾选“Copy all used libraries into the project folder”；
• 点击“Generate Code”，生成完成后点击“Open Project”直接打开Keil工程。

四、核心代码编写

4.1 头文件与全局变量定义（main.h）

#ifndef __MAIN_H
#define __MAIN_H

#include "stm32f1xx_hal.h"
#include "math.h"

/* 引脚宏定义 */
// NRF24L01控制引脚
#define NRF_CE_PIN GPIO_PIN_0
#define NRF_CE_GPIO_PORT GPIOB
#define NRF_CSN_PIN GPIO_PIN_1
#define NRF_CSN_GPIO_PORT GPIOB

/* 功能宏定义 */
// 舵机参数
#define SERVO_NUM 18 // 舵机总数
#define SERVO_MIN_PULSE 500 // 0°对应脉冲宽度（μs）
#define SERVO_MAX_PULSE 2500 // 180°对应脉冲宽度（μs）
#define SERVO_MID_PULSE 1500 // 90°中立位脉冲宽度（μs）
#define PCA9685_FREQ 50 // PWM频率50Hz（20ms周期）

// 六足步态参数
#define LEG_NUM 6 // 腿部数量
#define JOINT_NUM 3 // 每腿关节数
#define GAIT_FRAME_NUM 8 // 三角步态帧数
#define GAIT_DELAY 150 // 每帧延时（ms）

// 姿态调节参数
#define PITCH_OFFSET 0.0f // 俯仰角基准（水平）
#define ROLL_OFFSET 0.0f // 横滚角基准（水平）
#define KP 5.0f // PID比例系数
#define KI 0.1f // PID积分系数
#define KD 0.5f // PID微分系数

// 无线指令定义
#define CMD_STOP 0x00 // 停止
#define CMD_FORWARD 0x01 // 前进
#define CMD_BACKWARD 0x02 // 后退
#define CMD_LEFT 0x03 // 左转
#define CMD_RIGHT 0x04 // 右转

/* 数据类型定义 */
// 舵机角度结构体
typedef struct
{
uint8_t leg_id; // 腿部ID（0-5：左前/左中/左后/右前/右中/右后）
uint8_t joint_id; // 关节ID（0=髋，1=膝，2=踝）
uint16_t angle; // 目标角度（0-180°）
} Servo_AngleTypeDef;

// 姿态数据结构体
typedef struct
{
float pitch; // 俯仰角（°）
float roll; // 横滚角（°）
float yaw; // 偏航角（°）
} Attitude_TypeDef;

// 步态表结构体
typedef struct
{
uint8_t cmd; // 对应指令
uint16_t gait_table[LEG_NUM][JOINT_NUM][GAIT_FRAME_NUM]; // 步态角度表
} Gait_TypeDef;

/* 全局变量 */
extern I2C_HandleTypeDef hi2c1; // I2C1（PCA9685）
extern I2C_HandleTypeDef hi2c2; // I2C2（MPU6050）
extern SPI_HandleTypeDef hspi1; // SPI1（NRF24L01）

extern uint8_t g_current_cmd; // 当前无线指令
extern Attitude_TypeDef g_attitude; // 当前姿态数据
extern Servo_AngleTypeDef g_servo_target[SERVO_NUM]; // 舵机目标角度
extern float g_pid_pitch_out; // 俯仰角PID输出
extern float g_pid_roll_out; // 横滚角PID输出

/* 函数声明 */
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_I2C2_Init(void);
static void MX_SPI1_Init(void);

// PCA9685舵机驱动函数
void PCA9685_Init(uint8_t addr, uint16_t freq);
void PCA9685_Set_PWM(uint8_t addr, uint8_t channel, uint16_t pulse);
void Servo_Set_Angle(uint8_t servo_id, uint16_t angle);
void Servo_Set_Mid(void);

// MPU6050姿态检测函数
void MPU6050_Init(void);
void MPU6050_Get_Attitude(Attitude_TypeDef *att);

// PID控制函数
void PID_Calc_Pitch(float target, float current);
void PID_Calc_Roll(float target, float current);

// 六足步态规划函数
void Gait_Table_Init(void);
void Gait_Update(uint8_t cmd, uint8_t frame);
void Gait_Execute(void);

// NRF24L01无线通信函数
void NRF24L01_Init(void);
uint8_t NRF24L01_Receive(uint8_t *data);

// 姿态调节函数
void Attitude_Adjust(void);

#endif /* __MAIN_H */

4.2 外设初始化代码（main.c）

#include "main.h"

/* 全局变量定义 */
I2C_HandleTypeDef hi2c1;
I2C_HandleTypeDef hi2c2;
SPI_HandleTypeDef hspi1;

uint8_t g_current_cmd = CMD_STOP;
Attitude_TypeDef g_attitude = {0};
Servo_AngleTypeDef g_servo_target[SERVO_NUM] = {0};
float g_pid_pitch_out = 0;
float g_pid_roll_out = 0;

/* 系统时钟配置函数 */
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

/* 配置HSE外部晶振 */
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}

/* 配置系统时钟总线 */
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}

/* 配置I2C时钟源 */
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1|RCC_PERIPHCLK_I2C2;
PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
PeriphClkInit.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
}

/* I2C1初始化（PCA9685） */
static void MX_I2C1_Init(void)
{
hi2c1.Instance = I2C1;
hi2c1.Init.ClockSpeed = 100000;
hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
}

/* I2C2初始化（MPU6050） */
static void MX_I2C2_Init(void)
{
hi2c2.Instance = I2C2;
hi2c2.Init.ClockSpeed = 100000;
hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c2.Init.OwnAddress1 = 0;
hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c2.Init.OwnAddress2 = 0;
hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c2) != HAL_OK)
{
Error_Handler();
}
}

/* SPI1初始化（NRF24L01） */
static void MX_SPI1_Init(void)
{
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_MASTER;
hspi1.Init.Direction = SPI_DIRECTION_2LINES;
hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi1.Init.CRCPolynomial = 10;
if (HAL_SPI_Init(&hspi1) != HAL_OK)
{
Error_Handler();
}
}

/* GPIO初始化（NRF24L01控制） */
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};

/* 使能GPIO时钟 */
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();

/* 配置NRF24L01 CE/CSN引脚 */
GPIO_InitStruct.Pin = NRF_CE_PIN|NRF_CSN_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

/* 初始状态：CE低，CSN高 */
HAL_GPIO_WritePin(NRF_CE_GPIO_PORT, NRF_CE_PIN, GPIO_PIN_RESET);
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_SET);
}

/* 错误处理函数 */
void Error_Handler(void)
{
__disable_irq();
while (1)
{
// 错误时红灯闪烁（可外接LED）
HAL_Delay(500);
}
}

#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
}
#endif /* USE_FULL_ASSERT */

4.3 PCA9685舵机驱动函数（main.c续）

/* PCA9685寄存器定义 */
#define PCA9685_MODE1 0x00
#define PCA9685_MODE2 0x01
#define PCA9685_PRESCALE 0xFE
#define PCA9685_LED0_ON_L 0x06
#define PCA9685_LED0_ON_H 0x07
#define PCA9685_LED0_OFF_L 0x08
#define PCA9685_LED0_OFF_H 0x09

/* PCA9685初始化
addr：I2C地址（0x40/0x41）
freq：PWM频率（50Hz）
*/
void PCA9685_Init(uint8_t addr, uint16_t freq)
{
uint8_t data[2];
uint16_t prescale_val;

// 1. 进入睡眠模式
data[0] = PCA9685_MODE1;
data[1] = 0x10; // SLEEP=1
HAL_I2C_Master_Transmit(&hi2c1, addr<<1, data, 2, 100);

// 2. 设置预分频器（计算频率）
prescale_val = (uint16_t)(25000000.0f / (4096.0f * freq) – 0.5f);
data[0] = PCA9685_PRESCALE;
data[1] = prescale_val;
HAL_I2C_Master_Transmit(&hi2c1, addr<<1, data, 2, 100);

// 3. 退出睡眠，启用自动增量
data[0] = PCA9685_MODE1;
data[1] = 0x01; // AUTO_INCREMENT=1
HAL_I2C_Master_Transmit(&hi2c1, addr<<1, data, 2, 100);
HAL_Delay(5);

// 4. 设置输出模式为推挽
data[0] = PCA9685_MODE2;
data[1] = 0x04; // OUTDRV=1
HAL_I2C_Master_Transmit(&hi2c1, addr<<1, data, 2, 100);
}

/* PCA9685设置PWM脉冲宽度
addr：I2C地址
channel：通道号（0-15）
pulse：脉冲宽度（μs）
*/
void PCA9685_Set_PWM(uint8_t addr, uint8_t channel, uint16_t pulse)
{
uint8_t data[5];
uint16_t tick = (uint16_t)(pulse * 4096.0f / 20000.0f); // 20ms=20000μs，12位分辨率

// 寄存器地址：LEDn_ON_L = 0x06 + 4*channel
data[0] = PCA9685_LED0_ON_L + 4*channel;
data[1] = 0x00; // ON_L=0
data[2] = 0x00; // ON_H=0
data[3] = tick & 0xFF; // OFF_L
data[4] = (tick >> 8) & 0x0F; // OFF_H
HAL_I2C_Master_Transmit(&hi2c1, addr<<1, data, 5, 100);
}

/* 舵机角度设置
servo_id：舵机编号（0-17）
angle：目标角度（0-180°）
*/
void Servo_Set_Angle(uint8_t servo_id, uint16_t angle)
{
uint16_t pulse;
uint8_t addr, channel;

// 1. 角度限幅
if(angle > 180) angle = 180;
if(angle < 0) angle = 0;

// 2. 计算脉冲宽度（0°=500μs，180°=2500μs）
pulse = SERVO_MIN_PULSE + (uint16_t)((SERVO_MAX_PULSE – SERVO_MIN_PULSE) * angle / 180.0f);

// 3. 分配舵机到对应PCA9685通道
if(servo_id < 9)
{
addr = 0x40; // 第一块PCA9685
channel = servo_id; // 通道0-8
}
else
{
addr = 0x41; // 第二块PCA9685
channel = servo_id – 9; // 通道0-8
}

// 4. 设置PWM
PCA9685_Set_PWM(addr, channel, pulse);
}

/* 所有舵机设置为中立位（90°） */
void Servo_Set_Mid(void)
{
for(uint8_t i=0; i<SERVO_NUM; i++)
{
Servo_Set_Angle(i, 90);
HAL_Delay(10);
}
}

4.4 MPU6050姿态检测函数（main.c续）

/* MPU6050寄存器定义 */
#define MPU6050_ADDR 0x68
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C

/* DMP库相关（简化版，已封装核心逻辑） */
#define MPU6050_DMP_INIT 1
#define q0 1.0f
#define q1 0.0f
#define q2 0.0f
#define q3 0.0f

/* MPU6050初始化 */
void MPU6050_Init(void)
{
uint8_t data[2];

// 1. 唤醒MPU6050（解除睡眠）
data[0] = MPU6050_PWR_MGMT_1;
data[1] = 0x00; // SLEEP=0
HAL_I2C_Master_Transmit(&hi2c2, MPU6050_ADDR<<1, data, 2, 100);

// 2. 设置采样率（100Hz）
data[0] = MPU6050_SMPLRT_DIV;
data[1] = 0x09;
HAL_I2C_Master_Transmit(&hi2c2, MPU6050_ADDR<<1, data, 2, 100);

// 3. 设置低通滤波（5Hz）
data[0] = MPU6050_CONFIG;
data[1] = 0x06;
HAL_I2C_Master_Transmit(&hi2c2, MPU6050_ADDR<<1, data, 2, 100);

// 4. 设置陀螺仪量程（±2000°/s）
data[0] = MPU6050_GYRO_CONFIG;
data[1] = 0x18;
HAL_I2C_Master_Transmit(&hi2c2, MPU6050_ADDR<<1, data, 2, 100);

// 5. 设置加速度计量程（±8g）
data[0] = MPU6050_ACCEL_CONFIG;
data[1] = 0x10;
HAL_I2C_Master_Transmit(&hi2c2, MPU6050_ADDR<<1, data, 2, 100);

// 6. 初始化DMP（简化版，实际需引入DMP库）
HAL_Delay(100);
}

/* 获取姿态数据（俯仰/横滚/偏航） */
void MPU6050_Get_Attitude(Attitude_TypeDef *att)
{
// 简化版：通过四元数计算欧拉角
float pitch, roll, yaw;

// 实际项目中需读取DMP输出的四元数，此处为示例计算
pitch = asin(–2 * q1 * q3 + 2 * q0 * q2) * 57.3f;
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, –2 * q1 * q1 – 2 * q2 * q2 + 1) * 57.3f;
yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 – q2 * q2 – q3 * q3) * 57.3f;

att->pitch = pitch;
att->roll = roll;
att->yaw = yaw;
}

4.5 PID控制与姿态调节函数（main.c续）

/* PID参数结构体 */
typedef struct
{
float target; // 目标值
float current; // 当前值
float error; // 偏差
float last_error; // 上一次偏差
float integral; // 积分
float derivative; // 微分
float kp, ki, kd; // 系数
float output; // 输出
} PID_TypeDef;

PID_TypeDef pid_pitch = {0}, pid_roll = {0};

/* 俯仰角PID计算 */
void PID_Calc_Pitch(float target, float current)
{
pid_pitch.target = target;
pid_pitch.current = current;
pid_pitch.error = pid_pitch.target – pid_pitch.current;

// 积分限幅
pid_pitch.integral += pid_pitch.error;
if(pid_pitch.integral > 50) pid_pitch.integral = 50;
if(pid_pitch.integral < –50) pid_pitch.integral = –50;

// 微分
pid_pitch.derivative = pid_pitch.error – pid_pitch.last_error;

// PID输出
pid_pitch.output = pid_pitch.kp * pid_pitch.error +
pid_pitch.ki * pid_pitch.integral +
pid_pitch.kd * pid_pitch.derivative;

// 输出限幅（±10°补偿）
if(pid_pitch.output > 10) pid_pitch.output = 10;
if(pid_pitch.output < –10) pid_pitch.output = –10;

pid_pitch.last_error = pid_pitch.error;
g_pid_pitch_out = pid_pitch.output;
}

/* 横滚角PID计算 */
void PID_Calc_Roll(float target, float current)
{
pid_roll.target = target;
pid_roll.current = current;
pid_roll.error = pid_roll.target – pid_roll.current;

// 积分限幅
pid_roll.integral += pid_roll.error;
if(pid_roll.integral > 50) pid_roll.integral = 50;
if(pid_roll.integral < –50) pid_roll.integral = –50;

// 微分
pid_roll.derivative = pid_roll.error – pid_roll.last_error;

// PID输出
pid_roll.output = pid_roll.kp * pid_roll.error +
pid_roll.ki * pid_roll.integral +
pid_roll.kd * pid_roll.derivative;

// 输出限幅（±10°补偿）
if(pid_roll.output > 10) pid_roll.output = 10;
if(pid_roll.output < –10) pid_roll.output = –10;

pid_roll.last_error = pid_roll.error;
g_pid_roll_out = pid_roll.output;
}

/* 姿态调节：根据PID输出补偿舵机角度 */
void Attitude_Adjust(void)
{
// 初始化PID系数
pid_pitch.kp = KP;
pid_pitch.ki = KI;
pid_pitch.kd = KD;
pid_roll.kp = KP;
pid_roll.ki = KI;
pid_roll.kd = KD;

// 计算PID输出
PID_Calc_Pitch(PITCH_OFFSET, g_attitude.pitch);
PID_Calc_Roll(ROLL_OFFSET, g_attitude.roll);

// 补偿每腿踝关节角度（示例：左前腿-右后腿补偿）
for(uint8_t i=0; i<SERVO_NUM; i++)
{
// 踝关节ID：2,5,8,11,14,17
if(i == 2 || i == 5 || i == 8 || i == 11 || i == 14 || i == 17)
{
g_servo_target[i].angle += (uint16_t)g_pid_pitch_out;
g_servo_target[i].angle += (uint16_t)g_pid_roll_out;

// 角度限幅
if(g_servo_target[i].angle > 180) g_servo_target[i].angle = 180;
if(g_servo_target[i].angle < 0) g_servo_target[i].angle = 0;
}
}
}

4.6 六足步态规划函数（main.c续）

/* 三角步态表（前进）：每腿3个关节，8帧 */
const uint16_t gait_forward[LEG_NUM][JOINT_NUM][GAIT_FRAME_NUM] = {
// 左前腿（髋/膝/踝）
{{90,100,110,110,100,90,80,80}, {90,80,70,70,80,90,100,100}, {90,90,90,90,90,90,90,90}},
// 左中腿
{{90,80,70,70,80,90,100,100}, {90,90,90,90,90,90,90,90}, {90,100,110,110,100,90,80,80}},
// 左后腿
{{90,100,110,110,100,90,80,80}, {90,80,70,70,80,90,100,100}, {90,90,90,90,90,90,90,90}},
// 右前腿
{{90,80,70,70,80,90,100,100}, {90,90,90,90,90,90,90,90}, {90,100,110,110,100,90,80,80}},
// 右中腿
{{90,100,110,110,100,90,80,80}, {90,80,70,70,80,90,100,100}, {90,90,90,90,90,90,90,90}},
// 右后腿
{{90,80,70,70,80,90,100,100}, {90,90,90,90,90,90,90,90}, {90,100,110,110,100,90,80,80}}
};

/* 步态表初始化 */
void Gait_Table_Init(void)
{
// 初始化所有舵机目标角度为中立位
for(uint8_t i=0; i<SERVO_NUM; i++)
{
g_servo_target[i].leg_id = i / JOINT_NUM;
g_servo_target[i].joint_id = i % JOINT_NUM;
g_servo_target[i].angle = 90;
}
}

/* 更新步态帧角度 */
void Gait_Update(uint8_t cmd, uint8_t frame)
{
if(frame >= GAIT_FRAME_NUM) frame = 0;

switch(cmd)
{
case CMD_FORWARD:
// 加载前进步态表
for(uint8_t i=0; i<LEG_NUM; i++)
{
for(uint8_t j=0; j<JOINT_NUM; j++)
{
g_servo_target[i*JOINT_NUM + j].angle = gait_forward[i][j][frame];
}
}
break;

case CMD_BACKWARD:
// 后退步态：反转前进帧
for(uint8_t i=0; i<LEG_NUM; i++)
{
for(uint8_t j=0; j<JOINT_NUM; j++)
{
g_servo_target[i*JOINT_NUM + j].angle = gait_forward[i][j][GAIT_FRAME_NUM – frame – 1];
}
}
break;

case CMD_LEFT:
// 左转步态：左腿角度减小，右腿增大
for(uint8_t i=0; i<LEG_NUM; i++)
{
for(uint8_t j=0; j<JOINT_NUM; j++)
{
if(i < 3) // 左腿
g_servo_target[i*JOINT_NUM + j].angle = gait_forward[i][j][frame] – 10;
else // 右腿
g_servo_target[i*JOINT_NUM + j].angle = gait_forward[i][j][frame] + 10;
}
}
break;

case CMD_RIGHT:
// 右转步态：右腿角度减小，左腿增大
for(uint8_t i=0; i<LEG_NUM; i++)
{
for(uint8_t j=0; j<JOINT_NUM; j++)
{
if(i < 3) // 左腿
g_servo_target[i*JOINT_NUM + j].angle = gait_forward[i][j][frame] + 10;
else // 右腿
g_servo_target[i*JOINT_NUM + j].angle = gait_forward[i][j][frame] – 10;
}
}
break;

case CMD_STOP:
default:
// 停止：所有舵机中立位
for(uint8_t i=0; i<SERVO_NUM; i++)
{
g_servo_target[i].angle = 90;
}
break;
}
}

/* 执行步态：更新舵机角度 */
void Gait_Execute(void)
{
for(uint8_t i=0; i<SERVO_NUM; i++)
{
Servo_Set_Angle(i, g_servo_target[i].angle);
}
HAL_Delay(GAIT_DELAY);
}

4.7 NRF24L01无线通信函数（main.c续）

/* NRF24L01寄存器定义 */
#define NRF_CONFIG 0x00
#define NRF_EN_AA 0x01
#define NRF_EN_RXADDR 0x02
#define NRF_SETUP_AW 0x03
#define NRF_SETUP_RETR 0x04
#define NRF_RF_CH 0x05
#define NRF_RF_SETUP 0x06
#define NRF_STATUS 0x07
#define NRF_RX_ADDR_P0 0x0A
#define NRF_TX_ADDR 0x10
#define NRF_RX_PW_P0 0x11
#define NRF_FIFO_STATUS 0x17

/* NRF24L01写寄存器 */
void NRF24L01_Write_Reg(uint8_t reg, uint8_t data)
{
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, &reg, 1, 100);
HAL_SPI_Transmit(&hspi1, &data, 1, 100);
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_SET);
}

/* NRF24L01读寄存器 */
uint8_t NRF24L01_Read_Reg(uint8_t reg)
{
uint8_t data = 0;
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, &reg, 1, 100);
HAL_SPI_Receive(&hspi1, &data, 1, 100);
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_SET);
return data;
}

/* NRF24L01初始化（接收模式） */
void NRF24L01_Init(void)
{
HAL_Delay(100);

// 1. 配置基本参数
NRF24L01_Write_Reg(NRF_CONFIG, 0x0F); // 使能接收，CRC使能
NRF24L01_Write_Reg(NRF_EN_AA, 0x01); // 允许通道0自动应答
NRF24L01_Write_Reg(NRF_EN_RXADDR, 0x01); // 使能通道0
NRF24L01_Write_Reg(NRF_SETUP_AW, 0x03); // 地址宽度5字节
NRF24L01_Write_Reg(NRF_SETUP_RETR, 0x1A);// 重传延时500us，重传10次
NRF24L01_Write_Reg(NRF_RF_CH, 0x02); // 射频通道2
NRF24L01_Write_Reg(NRF_RF_SETUP, 0x07); // 功率0dB，速率1Mbps

// 2. 设置接收/发送地址（5字节，示例：0x01,0x02,0x03,0x04,0x05）
uint8_t addr[5] = {0x01,0x02,0x03,0x04,0x05};
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, (uint8_t*)(NRF_RX_ADDR_P0 | 0x20), 1, 100);
HAL_SPI_Transmit(&hspi1, addr, 5, 100);
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_SET);

// 3. 设置接收数据长度（1字节）
NRF24L01_Write_Reg(NRF_RX_PW_P0, 1);

// 4. 进入接收模式
NRF24L01_Write_Reg(NRF_CONFIG, 0x0F);
HAL_GPIO_WritePin(NRF_CE_GPIO_PORT, NRF_CE_PIN, GPIO_PIN_SET);
HAL_Delay(1);
}

/* NRF24L01接收数据 */
uint8_t NRF24L01_Receive(uint8_t *data)
{
uint8_t status = NRF24L01_Read_Reg(NRF_STATUS);

if((status & 0x40) == 0x40) // 接收到数据
{
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, (uint8_t*)0x61, 1, 100); // 读RX FIFO
HAL_SPI_Receive(&hspi1, data, 1, 100);
HAL_GPIO_WritePin(NRF_CSN_GPIO_PORT, NRF_CSN_PIN, GPIO_PIN_SET);

NRF24L01_Write_Reg(NRF_STATUS, 0x40); // 清除接收标志
return 1;
}
return 0;
}

4.8 主函数（main.c续）

/* 主函数 */
int main(void)
{
/* 初始化HAL库 */
HAL_Init();

/* 配置系统时钟 */
SystemClock_Config();

/* 初始化外设 */
MX_GPIO_Init();
MX_I2C1_Init();
MX_I2C2_Init();
MX_SPI1_Init();

/* 初始化功能模块 */
PCA9685_Init(0x40, PCA9685_FREQ); // 初始化第一块PCA9685
PCA9685_Init(0x41, PCA9685_FREQ); // 初始化第二块PCA9685
Servo_Set_Mid(); // 舵机中立位
MPU6050_Init(); // 姿态传感器初始化
NRF24L01_Init(); // 无线模块初始化
Gait_Table_Init(); // 步态表初始化

uint8_t gait_frame = 0;
uint8_t rx_data = 0;

/* 主循环 */
while (1)
{
// 1. 接收无线指令
if(NRF24L01_Receive(&rx_data))
{
g_current_cmd = rx_data;
}

// 2. 获取姿态数据
MPU6050_Get_Attitude(&g_attitude);

// 3. 姿态调节（PID补偿）
Attitude_Adjust();

// 4. 更新当前步态帧
Gait_Update(g_current_cmd, gait_frame);

// 5. 执行步态（更新舵机角度）
Gait_Execute();

// 6. 切换下一帧
gait_frame = (gait_frame + 1) % GAIT_FRAME_NUM;

// 7. 低功耗延时
HAL_Delay(10);
}
}

五、代码烧录与调试

5.1 代码编译与烧录

5.2 硬件调试步骤

步骤1：基础功能验证

• 上电检查：STM32上电后，所有舵机归位到90°中立位，MPU6050无异常发热，NRF24L01指示灯闪烁（接收模式）；
• 舵机调试：修改Servo_Set_Mid函数中角度为0/180，烧录后验证舵机是否能旋转到极限位置，无卡顿则驱动正常。

步骤2：姿态检测调试

• 手动倾斜机器人，通过Keil“Watch Window”查看g_attitude.pitch和g_attitude.roll值，倾斜时数值应在±30°范围内变化，水平时接近0°；
• 若姿态值无变化：检查MPU6050接线（SCL/SDA是否接反）、I2C地址是否为0x68（部分模块为0x69）。

步骤3：无线通信调试

• 遥控器端烧录NRF24L01发送代码（按键对应指令0x01-0x04），按下前进键，机器人端g_current_cmd应变为0x01；
• 若无线通信失败：核对NRF24L01的射频通道（RF_CH）、地址是否一致，CE/CSN引脚电平是否正确。

步骤4：步态运动调试

• 按下遥控器前进键，机器人应执行三角步态前进，6条腿交替抬升/落地，运动平稳无卡顿；
• 若步态异常：调整GAIT_DELAY（增大延时可降低速度）、步态表中角度值（避免舵机超程）；
• 姿态调节验证：倾斜机器人，PID输出应补偿舵机角度，使机器人快速恢复水平。

5.3 常见问题与解决

六、功能扩展建议

总结