简介这份平衡小车程序纯平衡压缩包面向嵌入式初学者、课程设计者及平衡车爱好者围绕STM32平台给出纯平衡控制的完整工程帮助读者理解从传感器采集、姿态解算到PID闭环驱动电机的实现路径。压缩包共234个文件、约8.57MB以h头文件、c源文件、o目标文件、crf交叉引用文件为主另含Keil工程配置、hex烧录文件及少量说明与脚本能较完整地保留编译与调试现场。已有2215人学习下载具备一定参考热度对想入门嵌入式控制的学习者较为友好。工程内含MPU6050/DMP相关驱动、定时器与ADC等外设配置、PID控制逻辑和电机输出模块适合对照学习STM32标准外设库编程、陀螺仪与加速度计数据融合、参数整定及实时控制思路也可作为课设、竞赛或自制平衡车的起步代码便于在其结构上修改机械参数与控制策略。1. 只保留平衡环的 STM32 小车砍掉扩展功能后反而更难调很多人第一次拆开平衡车代码动作都差不多先找蓝牙在哪再加块屏幕显示角度顺手把循迹也点上结果车连站都站不住。这份纯平衡的 STM32 工程反着来扩展功能全砍只留姿态解算、串级 PID 和电机输出三段main 里几行初始化剩下的全压在定时器中断里跑。它适合刚摸到嵌入式 PID 门槛、想把一个控制环从头到尾吃透的人代码量不大但每一行都挂在实时链路上改错一个系数车立刻倒给你看。也正因为砍得干净变量少、现象直接出了问题大概率就是你刚改的那一行。2. 控制链路拆解一个 5ms 周期里从陀螺仪读数跑到两路 PWM平衡车的代码看着零散实际上是一条很短的流水线定时器每 5ms 打一次节拍中断里读一次姿态、算三个 PID、写两路 PWM然后退出。整条链路跑完必须远小于 5ms因为下一拍很快就会来。把这条链拆开看你会发现问题几乎不会出在“算法看不懂”上而是出在各段的量纲、符号和时序对不对得上。2.1 定时器节拍决定一切为什么控制周期不能随便改倒立摆这种被控对象的机械摆动频率通常只有一两赫兹200Hz 的采样率完全够用所以 5ms 是一个很舒服的默认值。往上压到 1ms 未必更好六轴传感器走 I2C 读一次原始数据就要接近 1ms一旦总线被别的外设占住PID 这次就算不完输出会晚一拍现象是车莫名其妙地抽一下。往下放到 20ms角度环的微分项就变得很粗车会表现为反应迟钝、越晃越大。/* TIM3 更新中断周期 5ms整车的实时逻辑都挂在这里 */ void TIM3_IRQHandler(void) { if (TIM_GetITStatus(TIM3, TIM_IT_Update) ! RESET) { TIM_ClearITPendingBit(TIM3, TIM_IT_Update); g_tick_5ms; /* 低频任务用的软计时别在中断里做耗时的事 */ Balance_ControlLoop(); /* 整条控制链路预算 200us 以内 */ } }这段中断函数本身没什么技术含量关键在于它承担了“时钟源”的角色。g_tick_5ms是个全局计数器主循环里的串口上报、电池检测都靠它分频这样主循环里加多少东西都不会拖慢控制节拍。我一般会把控制周期的宏单独抽出来#define CTRL_PERIOD_S 0.005f /* 秒改周期时只改这一处 */互补滤波、积分项、微分项都要用到这个数如果各处硬编码0.005某天你把定时器改成 4ms姿态和 PID 会同时出现说不清的偏差排查起来相当折磨人。2.2 姿态解算互补滤波把陀螺仪和加速度计拧成一股绳陀螺仪积分出来的角度短期很准但零漂会随时间累加一分钟能漂好几度加速度计长期看是准的可车体一旦有振动输出就抖得厉害。互补滤波做的事很简单用陀螺仪负责高频用加速度计负责低频两条曲线加权拼起来。系数取 0.98 是嵌入式里最常见的经验值对应大约 0.25 秒的时间常数既能压住振动噪声又不会让角度跟得太慢。#define COMP_K 0.98f /* 互补系数越大越信陀螺仪越小越信加速度计 */ #define GYRO_SCALE 16.4f /* LSB/(deg/s)对应 ±2000dps 量程 */ float g_angle 0.0f; /* 融合后的俯仰角单位度 */ float g_gyro_bias 0.0f; /* 上电静止标定出来的零漂 */ float Get_Pitch(void) { float acc_angle, gyro_rate; /* 加速度计反算角度atan2 出来的单位是弧度乘 57.29578 转成度 */ acc_angle atan2f(acc_y, acc_z) * 57.29578f; /* 陀螺仪减去零漂再换算成 deg/s */ gyro_rate (gyro_y - g_gyro_bias) / GYRO_SCALE; /* 一阶互补滤波 */ g_angle COMP_K * (g_angle gyro_rate * CTRL_PERIOD_S) (1.0f - COMP_K) * acc_angle; return g_angle; }几个参数值得说明。GYRO_SCALE跟量程绑死量程从 ±2000dps 改成 ±500dps 时这个值要跟着换成 65.5忘了改的后果是角度变化速度被放大四倍车会剧烈前后抽。atan2f(acc_y, acc_z)里的坐标顺序取决于传感器在车上的贴装方向如果板子是竖着插的两个参数要对调符号也可能要取反。这一步最稳的验证方法是把车慢慢往前倾串口打印出来的角度应该单调变大中途不跳变、不变号。有人会问要不要上卡尔曼。纯平衡版本里两种实现都很常见卡尔曼效果确实更平滑但代码长、矩阵参数多一旦现象不对能查的变量反而更多。互补滤波只有一个COMP_K配合静态零漂校准对一辆只有两个轮子的小车完全够用调车阶段我更愿意留这种“一眼能看出谁在捣乱”的实现。2.3 串级 PID 的三层分工直立环、速度环、转向环这是整套代码里最容易写错的地方。很多新手把三个环的输出直接加起来送进电机结果车会缓慢加速跑掉——因为速度环的输出语义搞错了。正确的结构是串级直立环是内环直接决定 PWM速度环是外环它的输出不是 PWM而是叠加到直立环的目标角度上转向环独立只做左右轮差速。#define ANGLE_MID 0.5f /* 机械中值车真正立住时的角度必须实测 */ #define ANGLE_KP 30.0f #define ANGLE_KD 0.8f #define SPEED_KP 1.2f #define SPEED_KI 0.02f #define TURN_KP 8.0f #define TURN_KD 0.3f PID_t pid_angle, pid_speed, pid_turn; float g_target_angle, g_pwm_base, g_pwm_turn; void Balance_ControlLoop(void) { float angle Get_Pitch(); /* 当前倾角度 */ float gyro_y Get_GyroY(); /* 俯仰角速度度/秒 */ float speed (Encoder_SpeedL() Encoder_SpeedR()) * 0.5f; /* mm/s带符号 */ float turn Get_GyroZ(); /* 绕 Z 轴角速度度/秒 */ /* 外环速度环。车往前跑就把目标角度往后压让它自己减速 */ g_target_angle ANGLE_MID PID_Calc(pid_speed, 0.0f, speed); /* 内环直立环。目标角 - 实际角PD 输出直接给 PWM */ g_pwm_base PID_Calc(pid_angle, g_target_angle, angle); /* 转向环只产生差速不参与前后平衡 */ g_pwm_turn PID_Calc(pid_turn, 0.0f, turn); Motor_Output(g_pwm_base g_pwm_turn, g_pwm_base - g_pwm_turn); }ANGLE_MID是整份代码里最需要动手实测的参数它不是 0而是车靠自身重力站住时那块板子的真实倾角可能是 0.5 度也可能是 -1.8 度全看电池和电机怎么装。写错这个值车会稳定地朝一个方向匀速跑掉速度环的积分项要很久才能拉回来。符号方向也得说清楚。以“正角度等于前倾”为例车往前倾过头时angle target直立环误差为负输出的 PWM 为负这时候必须保证负 PWM 让轮子向前追。如果实测是往后退说明电机方向反了改接线或者改方向宏不要用把ANGLE_KP写成负数的方式绕过去——那样转向环的差速方向会跟着一起错。2.4 输出合成与死区补偿PWM 最后怎么落到两个轮子上算完的 PWM 是个浮点值可正可负直接丢给定时器比较寄存器之前还要过两道处理限幅和死区补偿。直流减速电机在低占空比下克服不了静摩擦占空比给 5% 它纹丝不动但电流照样在跑热量全堆在驱动芯片上。所以低于死区的非零值要么拉到死区要么干脆给 0别让它悬在中间。#define PWM_MAX 7200 /* 定时器 ARR7200对应 100% 占空比 */ #define MOTOR_DEAD_ZONE 380 /* 实测出来的起步占空比每台车不一样 */ static int16_t Constrain(int16_t v, int16_t lo, int16_t hi) { if (v hi) return hi; if (v lo) return lo; return v; } void Motor_Output(float left, float right) { int16_t pwm_l Constrain((int16_t)left, -PWM_MAX, PWM_MAX); int16_t pwm_r Constrain((int16_t)right, -PWM_MAX, PWM_MAX); /* 死区补偿非零但要不动直接抬到死区线上 */ if (pwm_l 0 pwm_l MOTOR_DEAD_ZONE) pwm_l MOTOR_DEAD_ZONE; if (pwm_l 0 pwm_l -MOTOR_DEAD_ZONE) pwm_l -MOTOR_DEAD_ZONE; if (pwm_r 0 pwm_r MOTOR_DEAD_ZONE) pwm_r MOTOR_DEAD_ZONE; if (pwm_r 0 pwm_r -MOTOR_DEAD_ZONE) pwm_r -MOTOR_DEAD_ZONE; /* 方向脚 占空比两路 H 桥分别控制左右轮 */ if (pwm_l 0) { AIN1_SET(); AIN2_CLR(); TIM_SetCompare1(TIM4, pwm_l); } else { AIN1_CLR(); AIN2_SET(); TIM_SetCompare1(TIM4, -pwm_l); } if (pwm_r 0) { BIN1_SET(); BIN2_CLR(); TIM_SetCompare2(TIM4, pwm_r); } else { BIN1_CLR(); BIN2_SET(); TIM_SetCompare2(TIM4, -pwm_r); } }MOTOR_DEAD_ZONE必须实测方法是写个开环测试占空比从 0 慢慢往上加记下轮子刚开始转动的那个值左右轮各测一遍取大的那个当统一死区。左右电机的机械特性不会完全一样差得多的时候可以给两边分别设死区车走直线的能力会明显好一些。3. 工程结构与核心代码从 main 到中断每层只干一件事拿到一个 zip先看目录比看代码有用。平衡车工程的分层逻辑其实非常固定硬件驱动一层、控制算法一层、应用逻辑一层、工程文件一层。分得清楚的工程改一个电机引脚只需要动一个宏分得乱的工程改引脚得全局搜索。3.1 目录分层一份能直接编译的 Keil 工程长什么样目录放什么改动频率User/main.c、中断服务函数、全局参数宏调参时天天改Hardware/电机、编码器、传感器、按键的驱动与引脚宏换硬件时改Control/互补滤波、PID_t结构体与PID_Calc算法升级时改BSP/定时器、PWM、串口、I2C 的初始化基本不动Project/Keil 工程文件、启动文件、分散加载基本不动这份工程的价值在于Control/是独立的里面没有任何一行代码去碰寄存器输入是角度和速度输出是 PWM 浮点值。这意味着你可以把整个Control/目录原样搬到另一个底盘上只换硬件驱动层参数重调一遍就能跑。3.2 main.c 只做三件事初始化、校准、开中断main里不该出现任何控制逻辑。它只负责把外设喂饱、把零漂标定完、把中断打开然后就进入一个以低频任务为主的死循环。int main(void) { /* 1. 时钟与基础外设顺序不能乱先时钟再外设 */ SystemClock_Config(); /* 72MHz 主频定时器和串口的波特率都依赖它 */ Delay_Init(); UART1_Init(115200); IMU_Init(); /* I2C 初始化 传感器量程配置 */ Encoder_Init(); Motor_Init(); /* PWM 定时器 方向脚 */ KEY_Init(); /* 2. 上电校准车必须放平且静止这两秒别碰桌子 */ IMU_Calibrate(); PID_Init(pid_angle, ANGLE_KP, 0.0f, ANGLE_KD, PWM_MAX, 0.0f); PID_Init(pid_speed, SPEED_KP, SPEED_KI, 0.0f, 20.0f, 300.0f); PID_Init(pid_turn, TURN_KP, 0.0f, TURN_KD, 1500.0f, 0.0f); /* 3. 开控制节拍 */ TIM3_Init(5000); /* 5ms 更新中断 */ g_enable 1; while (1) { /* 主循环只放非实时任务20ms 一轮 */ if (g_tick_5ms % 4 0) { Battery_Update(); Debug_SendScope(); /* 波形上报注意别用阻塞 printf 拖太久 */ } KEY_Scan(); /* 按键启停出事了能一键停机 */ } }三个PID_Init的最后一个参数是积分限幅pid_speed给了 300意思是角度补偿最多累加到 300 个单位防止车被手按住时积分项一直涨、松手瞬间窜出去。这个值设多大要结合SPEED_KI一起看ki乘的是积分累加和所以ki通常只有 0.001~0.05 这个量级看到有人写SPEED_KI 5.0基本可以断定单位搞混了。3.3 中断服务函数所有实时逻辑都在这里中断函数里做的事情越少越好但该做的必须一次做完。顺序上先读传感器、再算姿态、再算 PID、最后输出中间不能有任何阻塞调用。判断有没有阻塞最直接的办法是在函数开头拉高一个空闲 GPIO结尾拉低用示波器看这个引脚的高电平时间超过 1ms 就得拆。void Balance_ControlLoop(void) { static uint8_t fault_cnt 0; /* 单次读取失败不要立刻停机连续失败才认为传感器掉了 */ if (IMU_ReadRaw() ! 0) { if (fault_cnt 20) { Motor_Stop(); g_enable 0; } return; } fault_cnt 0; /* 角度超过 35 度说明已经扶不住了直接断电保护电机和驱动 */ if (fabsf(Get_Pitch()) 35.0f) { Motor_Stop(); g_enable 0; return; } if (g_enable) Balance_ControlLoop(); else Motor_Stop(); }这段保护逻辑在调车阶段特别有用。参数调错的时候车会疯狂输出电机堵转几分钟就能把驱动芯片烧了加一个角度阈值判断车一倒就掐断输出能省下不少返修时间。fault_cnt那种累计计数也不可少I2C 偶发一次读失败很正常直接停机反而会让车在路上莫名熄火。3.4 PID 实现细节位置式、积分限幅与微分先行平衡车用位置式 PID 就够了增量式在这个场景没什么优势。真正值得注意的是两处细节积分限幅和微分先行。微分如果对误差求目标角度被速度环一改微分项就会跳出一个尖峰车会“咯噔”一下改成对测量值求微分目标突变就不会冲击输出了。typedef struct { float kp, ki, kd; float integral; /* 误差累加和 */ float last_measure; /* 上一次的测量值用于微分先行 */ float out_max; /* 输出限幅 */ float integral_max; /* 积分限幅 */ float output; } PID_t; void PID_Init(PID_t *p, float kp, float ki, float kd, float out_max, float integral_max) { p-kp kp; p-ki ki; p-kd kd; p-integral 0.0f; p-last_measure 0.0f; p-out_max out_max; p-integral_max integral_max; p-output 0.0f; } float PID_Calc(PID_t *p, float target, float measure) { float err target - measure; /* 积分累加 限幅防止长时间卡死后的积分饱和 */ p-integral err; if (p-integral p-integral_max) p-integral p-integral_max; if (p-integral -p-integral_max) p-integral -p-integral_max; /* 微分先行对测量值求微分前面带负号 */ p-output p-kp * err p-ki * p-integral - p-kd * (measure - p-last_measure); p-last_measure measure; if (p-output p-out_max) p-output p-out_max; if (p-output -p-out_max) p-output -p-out_max; return p-output; }ki * integral里没有乘采样周期所以ki的物理含义是“每累加一个单位的误差贡献多少输出”单位不直观但也正因为如此改控制周期时不用重调ki——这一点比标准形式省心。kd项用的是相邻两次测量值之差同样没除周期等效于把微分增益吸收进了系数里调参时按数量级增减就行。4. 让它站起来编译配置、零漂校准与分环调参的完整顺序代码能编译通过只是第一步真正花时间的是让车稳住。这一步没有捷径但有顺序先保证机械和传感器是对的再单独调直立环稳了之后加速度环最后才是转向。顺序乱了你会同时面对三个变量任何一个现象都推不出原因。4.1 编译环境与工程配置下载前值得检查的五项用 Keil MDK 打开Project/下的工程文件编译前把这几项过一遍能省掉大量“编译过了但不跑”的时间。器件型号要和板子对上选错型号最常见的结果是下载成功但外设全不工作。外部晶振频率宏要和板上实际晶振一致8MHz 写成 12MHz串口会出一堆乱码定时器周期也会整体偏掉。C99 模式要打开工程里用了不少中途声明变量的写法老标准会直接报错。下载方式选对SWD 要比 JTAG 少占引脚调试口被复用成 GPIO 是很常见的问题。优化等级调参阶段建议先用-O0优化开高了单步调试会跳行排查问题时容易怀疑人生。下载之后先别急着让车跑把车架起来让轮子悬空串口打印角度和 PWM用手转动车体看数值跟不跟得上。这一步能提前发现九成的接线和量程问题。4.2 上电前的机械中值标定与陀螺仪零漂校准零漂标定的原理很朴素车静止的时候陀螺仪输出的理论值应该是 0实际读到的那个非零常数就是零漂。采 500 次求平均5ms 一次大约 2.5 秒够稳定了。#define CALI_SAMPLES 500 /* 500 × 5ms ≈ 2.5s */ void IMU_Calibrate(void) { int32_t sum_gyro_y 0; float sum_angle 0.0f; for (int i 0; i CALI_SAMPLES; i) { IMU_ReadRaw(); sum_gyro_y raw_gyro_y; sum_angle atan2f(acc_y, acc_z) * 57.29578f; Delay_ms(5); } g_gyro_bias (float)sum_gyro_y / CALI_SAMPLES; /* 零漂 */ g_angle sum_angle / CALI_SAMPLES; /* 静止初值 */ /* 把 g_angle 打到串口上它就是 ANGLE_MID 的参考值 */ printf(bias%.2f angle%.2f\r\n, g_gyro_bias, g_angle); }标定过程有两个硬性要求车必须放平、必须静止手不能搭在车上桌子也不能晃。我有一个血泪经验是标定时旁边有人走动地板轻微振动标出来的零漂偏了 0.6车能站但一直缓慢往一个方向漂查了半天以为是机械中值的问题。打印出来的angle就是ANGLE_MID的参考值把它填进宏里重新编译。也有人图省事让程序每次上电自动把当前角度当机械中值这在地面绝对水平的环境里能用但只要上电时车被放在有坡度的地方程序就会记住那个歪掉的角度然后一直往坡下追。4.3 分环调试先直立、再速度、最后转向三个环一起开是调不出来的。把不参与调试的环的输出直接置零逐个击破。阶段开哪个环参数起点合格标准一仅直立环ANGLE_KP15ANGLE_KD0.3手扶起车后能自己站 3 秒以上前后小幅摆动二直立 速度环SPEED_KP0.5SPEED_KI0.005用手轻推车能回到原位不持续加速跑掉三三个环全开TURN_KP5TURN_KD0.1给一个转向指令车能原地转且不前后窜第一阶段最容易出问题。ANGLE_KP从小往大加加到车能明显感觉到“有人在扶”但开始高频抖动时退回上一档然后加ANGLE_KD压抖动。KD不是越大越好超过某个点车会发出“嗡嗡”的高频声那是微分项在放大传感器噪声这时候要把KD减半。第二阶段的速度环只调KP先让车被推动后能慢慢回到原点再补一点点KI消掉静差。SPEED_KI一上来就给大值现象是车站在原地前后小幅来回“点头”那是积分项跑过头了直接砍掉一半再看。4.4 用串口波形看数据判断是参数问题还是硬件问题光靠眼睛看车晃很难区分是角度算错了还是参数不对。把关键变量以固定格式打到串口用上位机画成曲线三条线一起看判断起来快很多。/* 放在主循环里20ms 一次绝对不能塞进 5ms 中断printf 是阻塞的 */ void Debug_SendScope(void) { printf(%.2f,%.2f,%.2f,%d\r\n, g_angle, /* 实际角度 */ g_target_angle, /* 速度环改写后的目标角度 */ g_pwm_base, /* 直立环输出的基础 PWM */ (int)Encoder_SpeedL()); }看曲线有几个固定的判据。角度曲线如果本身就是毛刺说明传感器读数或滤波有问题先别调 PID角度平滑但 PWM 一会儿顶到正限幅一会儿顶到负限幅那是ANGLE_KP太大目标角度曲线缓慢往一个方向爬那是速度环积分在积累车确实在单向漂。这套东西相当于给车装了个黑匣子出问题回看几秒数据比盯着车猜有用得多。5. 避坑与排查调平衡车时最容易翻车的五个现场5.1 上电抖动、站不住、往一个方向跑现象一上电就高频抖动轮子发出“哒哒”声越抖越厉害直到倒下。原因基本是微分项在放大传感器噪声或者陀螺仪零漂没标定就开环跑了。判断方法很简单把车静置不动串口看陀螺仪原始值正常应该在 ±2 以内超过 ±10 说明量程配置或者通信有问题。解决顺序是先重做一遍静止校准再把ANGLE_KD减半最后检查COMP_K是不是被改小了COMP_K越小加速度计的振动越容易进来。现象二车能站住但一直往一个方向缓慢加速直到撞墙。这是ANGLE_MID不准的典型表现也可能是直立环本身存在静差。方法是用手扶着车让它刚好能自己站住串口读那个角度直接填进ANGLE_MID每次微调 0.1 度。如果调完还是在漂就把速度环的SPEED_KI加上去用积分把这点静差吃掉。现象三手一推车轮子朝反方向跑车瞬间倒地。直立环符号反了。检验标准只有一条车往前倒的时候轮子必须往前追。反了就去改电机方向宏别改KP的正负号因为转向环的差速方向跟电机方向是绑在一起的改错了一个另一个必然跟着错。5.2 电机发热、驱动发烫、加了外设就不稳现象四车能跑但电机和驱动芯片很快发烫续航也明显变短。通常是死区补偿没做或者做得不对。占空比给 5% 时电机不转但电流一直流热量全堆在 H 桥上。实测每台车的起步占空比填进MOTOR_DEAD_ZONE同时确认限幅PWM_MAX没有超过定时器的 ARR。还有一种可能是方向脚切换时没有留死区时间同一路 H 桥上下管瞬间直通这个只能靠示波器看两路驱动波形是否重叠。现象五加了屏幕显示或者无线模块后车就开始抖去掉就正常。典型的资源抢占。I2C 读屏是阻塞操作一次刷屏几百微秒正好卡在控制中断里节拍就乱了。解决办法是把显示刷新降到 5Hz并且只在主循环里刷串口调试同理printf一定要放在主循环或者改成 DMA 发送。中断里只做读传感器、算 PID、写 PWM 这三件事其他统统赶出去。6. 二次开发把纯平衡版本改成能远程调参、会自我保护的底盘纯平衡版本跑稳之后大部分人下一步都会想把参数整定做得舒服一点。因为调一次参数就编译下载一次一天下来能把 Flash 擦写几十遍效率极低。我一般会给它加一条极简的串口命令通道收到特定字符串就改某个参数改完立刻生效这样调车时电脑放在旁边边推车边改数。/* 主循环里轮询格式P,角度环KP,850 - 把 pid_angle.kp 改成 0.85 */ void Cmd_Parse(const char *line) { char name[4]; float value; if (sscanf(line, %3[^,],%f, name, value) ! 2) return; if (name[0] A name[1] P) pid_angle.kp value; /* AP */ else if (name[0] A name[1] D) pid_angle.kd value; else if (name[0] S name[1] P) pid_speed.kp value; else if (name[0] S name[1] I) pid_speed.ki value / 1000.0f; else if (name[0] M name[1] D) g_angle_mid value; /* 改完参数顺手清零积分避免旧积分和新参数打架 */ pid_speed.integral 0.0f; printf(OK %s%.3f\r\n, name, value); }这段代码的关键不在解析而在最后那行清零积分。调参时把SPEED_KI调大如果旧积分还留着车会立刻窜一下很容易误判成参数给大了。MID参数改完也建议停机重开一次让姿态从新的中值重新收敛。另一个值得加的是分级的保护策略。单个角度阈值太粗暴车在正常颠簸时也会误触发我一般分两档超过 25 度先做输出限幅把 PWM 压到一半超过 35 度直接停机并锁死必须按按键才能恢复。锁死这个设计是必须的自动恢复的后果是车在地上疯狂抽搐把轮胎和齿轮磨平。#define ANGLE_WARN 25.0f #define ANGLE_STOP 35.0f void Safety_Check(float angle) { if (fabsf(angle) ANGLE_STOP) { Motor_Stop(); g_enable 0; /* 锁死等按键复位 */ g_fault FAULT_FALL; return; } if (fabsf(angle) ANGLE_WARN) g_out_scale 0.5f; /* 输出打折先保命 */ else g_out_scale 1.0f; }再往后想扩展比如让它按指令走直线、定距离停车就轮到编码器里程闭环上场了。做法是在速度环外面再套一层位置环把目标速度改成由位置误差算出来的值SPEED_KP那一层完全不用动。这个结构的好处是位置环可以给得很慢慢到几乎不影响平衡调起来不会把已经稳定的直立环搞崩。最后说个自己的习惯每次改完参数我一定先把车架起来空转十秒看串口打印的角度和 PWM 是否在合理范围再放到地上跑。从那以后我每次烧录新固件都不会直接上手扶车试而是先让轮子悬空跑一遍确认没有满占空比输出、没有角度跳变再落地。这套流程看着慢实际上比摔坏一次电机省时间得多。希望帮到你。本文还有配套的精品资源点击获取