目录

1.通过LED灯实时监控电池电压
2.计算电机参数（旋转角度与旋转方向）
3.双电机PWM控制
4.PID 闭环调速
5.编码器T法测速
6.MPU6050 姿态解算
7.速度环设定和倒立摆动力学解算
8.平衡车串口遥控控制

1------通过LED灯实时监控电池电压

1. 初始化模块
   整体初始化（App_Bat_Init）：依次初始化 TIM2_TRGO（ADC 触发源）、ADC1（电压采集）、LED（电量指示）。
   LED 初始化（LED_Init）：配置 PA4/PA5/PA6 为推挽输出模式，用于驱动三颗电量指示 LED。
   TIM2_TRGO 初始化（TIM2_TRGO_Init）：
   TIM2 定时器配置为 10ms 定时（72MHz 主频，预分频 71 + 重装载值 9999）；
   选择 Update 事件作为 TRGO 触发源，为 ADC 注入转换提供外部触发信号。
   ADC1 初始化（ADC1_Init）：
   配置 PB0（ADC 通道 8）为模拟输入模式，用于采集电池电压；
   ADC1 配置为注入通道模式，由 TIM2_TRGO 触发转换，采样通道 8；
   开启 ADC 注入转换完成中断（JEOC），并配置 NVIC 中断优先级。
2. 电压采集与计算（ADC1_2_IRQHandler）
   ADC 注入转换完成后触发中断，读取注入通道 1 的转换值（12 位 ADC，范围 0~4095）；
   电压换算公式：vbat = 转换值/4095.0f * 3.3V * (8.4/3.3)（3.3V 为 ADC 参考电压，8.4/3.3 为电压分压比修正）；
   换算结果存入全局变量vbat，供后续电量判断使用。
3. 电量指示逻辑（App_Bat_Proc）
   按vbat值分级控制 LED，执行频率由外部任务调度决定：
   电压范围 LED 状态（PA4/PA5/PA6） 电量等级

7.9V 全亮 满电
7.4V~7.9V 前两颗亮，第三颗灭 75%
7.0V~7.4V 第一颗亮，后两颗灭 50%
≤7.0V（代码 BUG） 全灭（逻辑错误） 20%
低电压（else） 100ms 间隔闪烁（全亮 / 全灭切换） 低电报警

2------计算电机参数（旋转角度与旋转方向）

1.编码器硬件初始化：
左编码器：配置 PB14（A 相）、PB15（B 相）为上拉输入，通过 EXTI15_10 中断捕获 A 相高低电平变化；
右编码器：关闭 JTAG、开启 SWD 释放 PB3/PB4 引脚，配置为上拉输入，通过 EXTI3 中断捕获 A 相电平变化；
中断优先级均设为最高（抢占 0、子优先级 0），保证脉冲捕获无延迟。

2.旋转角度采集：
中断中累计编码器脉冲数（正转 + 1、反转 - 1），通过固定传动比公式将脉冲数换算为轮胎旋转角度（单位：度），支持左右路独立读取。

3.角速度计算：
记录脉冲间隔时间戳，取 “历史间隔” 与 “当前间隔” 的较小值作为周期，结合旋转方向和机械参数，换算为角速度（单位：rad/s），方向突变时返回 0 避免计算异常。

4.旋转方向判断：
通过编码器 A/B 相电平组合判断正 / 反转，用direction（±1/±2）标记 “稳定转向”（±1）和 “方向突变”（±2），提升转速计算鲁棒性。

物理量 计算公式 参数说明
旋转角度 脉冲数 / 22 / (30613/1500) × 360 22、30613/1500 为机械传动比
角速度 方向 / 脉冲周期 (T) / 22 / (30613/1500) × 2π（6.2831853） T 取微秒级时间间隔，转化为秒单位

3------双电机PWM控制

功能 实现方式
1.休眠 / 唤醒 App_PWM_Cmd：0→PA1 低电平（休眠），非 0→PA1 高电平（活动）

2.左电机转向 + 占空比
提取占空比符号（正 / 负），正数→AIN1 低、AIN2 高（正转），负数→AIN1 高、AIN2 低（反转）；
取占空比绝对值，换算为 TIM1_CH1 的 CCR 值（0~999），设置 PWM 占空比

3.右电机转向 + 占空比
提取占空比符号（正 / 负），正数→BIN1 高、BIN2 低（正转），负数→BIN1 低、BIN2 高（反转）；
取占空比绝对值，换算为 TIM4_CH1 的 CCR 值（0~999），设置 PWM 占空比

4------PID 闭环调速

1.PID 控制器初始化：为左右电机分别创建独立的 PID 控制器，配置固定参数（P=0.5、I=7、D=0），并限制 PID 输出范围为 ±8.4V（匹配电池电压上限），确保驱动电压不超量程。

2.闭环调速逻辑（1ms 周期）：
转速反馈：读取编码器模块的左右电机实时角速度（rad/s）；
PID 计算：对比目标转速（设定值）与实际转速（反馈值），输出需施加的电机驱动电压；
电压适配：结合当前电池电压，将驱动电压换算为 PWM 占空比（±100% 范围）；
驱动输出：将占空比下发至 PWM 驱动模块，控制电机转速。

3.转速与启停控制：
提供接口设置左右电机的目标转速（rad/s）；
提供电机整体启停接口，启停时复位 PID 控制器，避免积分饱和、转速突变等异常。

*****核心流程（App_Motor_Proc，1ms 执行一次）
目标转速(SetPoint) → PID计算 → 驱动电压(Ua) → 结合电池电压换算占空比 → PWM输出 → 编码器反馈实际转速 → 闭环修正

函数 功能说明 关键参数 / 特性
App_Motor_Init ：初始化 PID 控制器 P=0.5、I=7、D=0，输出限幅 ±8.4V
App_Motor_SetOmega_L/R ：设置左右电机目标转速 参数：Omega（rad/s），修改 PID 设定值
App_Motor_Cmd ：电机启停控制 非 0→激活 PWM，0→休眠；同步复位 PID
App_Motor_Proc ：调速核心逻辑（1ms 周期） 编码器反馈→PID 计算→占空比输出

5------编码器T法测速

ω=1/T÷每圈线数÷减速比∗360
通过算法解决速度不归零和换向波形异常
1.先猜测速度与上一周期相同：ω=direction/t0−t1 ÷22÷20.4∗360–>ω=direction/max(t0−t1 , now−t0) ÷22÷20.4∗360
2.电机转向改变时Direction特别标注

通过预先关闭中断解决毛刺问题

6------MPU6050 姿态解算

1.硬件初始化：完成 I2C 总线（PB8/PB9 重映射）和 MPU6050/6500 的参数配置，包括传感器复位、睡眠模式关闭、陀螺仪 / 加速度计量程设定。

2.原始数据采集：读取加速度计（X/Y/Z 轴，单位 g）、陀螺仪（X/Y/Z 轴，单位 °/s）、温度传感器（单位℃）的原始数据，并换算为物理量。

3.姿态解算：每 5ms 执行一次互补滤波，融合陀螺仪积分值（短期精准）和加速度计解算值（长期稳定），输出平滑的欧拉角（yaw/pitch/roll，单位 °）。

4.数据接口：提供各轴加速度、角速度、温度、欧拉角的读取接口，供上层模块调用。

硬件配置
模块 配置细节
I2C 总线 PB8/PB9 重映射为 I2C1 引脚，配置为开漏复用输出，通信速率 400kHz（高速模式）
MPU6050/6500 陀螺仪量程 ±2000°/s，加速度计量程 ±2g；复位后关闭睡眠模式，激活传感器

每 5ms 执行一次，融合逻辑如下：
1.陀螺仪积分预测：基于角速度积分更新欧拉角（短期无漂移）：
yaw = yaw + gz×0.005（z 轴角速度 ×5ms）
pitch = pitch + gx×0.005（x 轴角速度 ×5ms）
roll = roll - gy×0.005（y 轴角速度 ×5ms，符号适配安装方向）

---

2.加速度计解算修正：通过重力分量解算俯仰角 / 翻滚角（长期稳定）：
pitch_a = arctan2(ay, az) × (180/π)
roll_a = arctan2(ax, az) × (180/π)

---

3.互补滤波融合：权重 95.238% 给陀螺仪（保证动态响应），4.762% 给加速度计（修正漂移）：
pitch = 0.95238× 陀螺仪预测值 + 0.04762× 加速度计解算值
roll = 0.95238× 陀螺仪预测值 + 0.04762× 加速度计解算值
yaw 仅依赖陀螺仪（加速度计无法解算偏航角）

7------速度环设定和倒立摆动力学解算

1.数据采集与单位转换：
编码器：取左右轮平均角速度（omega），单位 rad/s；
MPU6050：俯仰角（theta）、X 轴角速度（theta_dot）、Z 轴角速度（gz），均转换为 rad rad/s 单位；

2.速度环反馈计算：
基于倒立摆模型，分离车体 “倾斜带来的速度分量” 和 “实际移动速度分量”，得到车体实际移动速度（x_dot，单位 m/s）；

3.多环 PID 解算：
速度环：对比 “设定速度” 与 “实际速度”，输出目标倾角（theta_ref），使车体通过倾斜产生加速度以匹配设定速度；
角度环：对比 “目标倾角” 与 “实际倾角”，输出目标角速度；
角速度环：对比 “目标角速度” 与 “实际角速度”，输出目标角加速度；

4.倒立摆逆运算：
根据刚体动力学公式，将目标角加速度转换为车体所需的线加速度（x_dot_dot_ref）；

5.电机转速积分：
对线加速度积分得到电机目标转速（omega_ref），结合转向环输出的轮速差（omega_diff），分别设置左右电机目标转速；

6.转向控制：
通过 Z 轴角速度（gz）反馈，调节左右轮速差，实现车体转向的精准控制。

核心控制逻辑（5ms 周期执行）
速度设定值 → 速度环PID → 目标倾角 → 角度环PID → 目标角速度 → 角速度环PID → 目标角加速度→ 倒立摆动力学解算 → 车体目标加速度 → 积分得到电机目标转速 → 转向环修正左右轮速差 → 输出至电机

8------平衡车串口遥控控制

1.串口初始化：配置 USART3（PB10/TX、PB11/RX）为 9600 波特率的串口通信模式，开启接收中断，实现指令的实时接收。

2.中断式指令接收：通过 USART3_RXNE 中断逐字节接收串口数据，换行符（\n）作为指令结束标志，完成一行指令的缓存与标记。

3.指令解析与执行：解析 “move 转向值 速度值” 格式指令，换算为平衡车控制参数，调用底层控制接口实现速度和转向控制。

4.数据安全缓存：采用三段式缓冲区（中断缓冲区、转运缓冲区、进程缓冲区），避免中断与主进程的数据竞争，保证指令解析安全。

硬件配置
模块 配置细节
USART3 PB10（TX，复用推挽输出）、PB11（RX，上拉输入），9600 波特率、8 位数据位、1 位停止位、无校验
中断配置 USART3_RXNE 中断（接收非空），抢占优先级 0、子优先级 0，保证指令接收无延迟

项目总结

1. 硬件驱动（底层基础）
   1.编码器模块：通过 EXTI 中断捕获左右电机编码器脉冲，计算旋转角度 / 角速度（rad/s），为电机调速、平衡控制提供转速反馈；
   2.MPU6050 模块：I2C 通信读取加速度计 / 陀螺仪数据，通过互补滤波融合出稳定的欧拉角（俯仰 / 翻滚 / 偏航），为平衡控制提供姿态反馈；
   3.PWM/TB6612 模块：TIM1/TIM4 生成 72kHz PWM 信号，控制 TB6612 驱动芯片实现左右电机转向 / 占空比调节，执行调速指令；
   4.电池电压检测模块：TIM2 触发 ADC1 采集电池分压电压，换算为实际电压，为电机 PWM 占空比动态适配提供依据；
   5.串口遥控模块：USART3 中断接收串口指令，解析 “move 转向 速度” 指令，转换为平衡控制参数。
2. 基础控制（调速与状态监控）
   电机 PID 调速模块：1ms 周期执行，读取编码器转速反馈，通过 PID 算法计算驱动电压，结合电池电压换算 PWM 占空比，实现电机精准调速；
   电池监控模块：10ms 周期采集电压，通过 LED 分级指示电量，低电压时闪烁报警。
3. 核心控制（自平衡核心）
   多环 PID 平衡控制：5ms 周期执行，采用 “速度环→角度环→角速度环→转向环” 四层 PID 架构，结合倒立摆动力学模型，将速度 / 转向指令转换为左右电机目标转速，实现车体自平衡；
   关键逻辑：速度环定行进速度→角度环调车体倾角→角速度环稳姿态→转向环控左右轮速差，最终输出至电机调速模块。

作者结语

第一次写有诸多不完美的地方多多包涵，如有错误或者能优化的地方请指出来。此文章基于铁头山羊平衡车，如需要了解更详细的，请前往B站深入学习，当然该文章结合视频效果更佳。

---

**
**
**

禁止转载
如有侵权，请及时联系我