简介一份基于WHEELTEC B570平衡小车的完整嵌入式课程设计工程代码与文档说明覆盖STM32F1系列HAL库驱动、MPU6050姿态解算、定时器/ADC/UART等外设配置适合正在学习STM32或需要快速完成课程项目的读者参考。压缩包共315个文件约1.13MB主要包含C源码与头文件h/c、编译中间文件o/d/map、构建配置mk/makefile/ld及CubeMX工程ioc/mxproject等附带清晰的说明文档目录结构完整。目前已有257人学习下载代码注释详细新手也能对照理解。部署简单烧录后即可配合小车进行平衡调试既能作为期末大作业直接提交也适合作为综合实践项目的功能框架从中可以学习到外设初始化、传感器数据融合、控制逻辑与嵌入式工程组织的实际经验内容覆盖课程设计中从底层驱动到应用逻辑的主要环节。1. B570平衡小车嵌入式作业代码不是烧录跑通而是把控制链路讲清楚WHEELTEC B570 平衡小车套件跑通 demo 很容易但要让这份嵌入式作业代码在课程设计里拿到高分得先回答一个问题stm32f1xx_hal_tim.c、stm32f1xx_hal_uart.c、inv_mpu.c、inv_mpu_dmp_motion_driver.c 这些源文件在整车控制链路里各扮演什么角色。B570 的平衡原理并不神秘本质是倒立摆模型加姿态反馈难点在于让新手在有限周期内把时钟树、PWM 输出、编码器回读、MPU6050 DMP 解算和串级 PID 完整串起来。这篇以 B570 平台的平衡小车嵌入式作业代码为主线从 HAL 外设初始化到最终文档编排给出可直接复现的步骤和参数。2. 反推硬件拓扑从源码文件映射 B570 外设与 HAL 初始化2.1 先看文件清单再对照官方原理图拿到项目代码不要直接打开 main.c 一路往下读。我一般先把源文件列表当作地图判断这块板子到底干了哪些事stm32f1xx_hal_rcc.c / stm32f1xx_hal_rcc_ex.c时钟树配置系统主频和外设总线时钟都由它决定stm32f1xx_hal_tim.c / stm32f1xx_hal_tim_ex.c定时器初始化B570 上电机 PWM 和编码器输入捕获都在这里stm32f1xx_hal_adc.c / stm32f1xx_hal_adc_ex.c电池电压采样低电量时限制 PWM 输出防止 PID 失控stm32f1xx_hal_uart.c串口透传用于蓝牙遥控和调试数据下发inv_mpu.c / inv_mpu_dmp_motion_driver.cMPU6050 驱动与 DMP 固件加载姿态解算的核心这和 B570 的硬件构成吻合主控是 STM32F103 系列电机驱动部分一般是 TB6612姿态传感器用 MPU6050两路直流减速电机带霍尔编码器。把这个映射关系理清楚就明白源代码为什么拆成这些文件——它本身就是「外设驱动 传感器融合 控制算法」三件套的标准分层。2.2 时钟树和 GPIO 复用是第一个坑看 stm32f1xx_hal_rcc.c 时重点观察__HAL_RCC_GPIOA_CLK_ENABLE()、__HAL_RCC_TIM2_CLK_ENABLE()这类宏。F103 外设时钟默认不全部打开漏一个使能初始化直接进 Error_Handler。下面这段代码是 B570 工程里常见的 PWM 初始化片段void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim) { GPIO_InitTypeDef GPIO_InitStruct {0}; if (htim-Instance TIM1) { __HAL_RCC_TIM1_CLK_ENABLE(); __HAL_RCC_GPIOA_CLK_ENABLE(); GPIO_InitStruct.Pin GPIO_PIN_8 | GPIO_PIN_11; // CH1 左电机, CH4 右电机 GPIO_InitStruct.Mode GPIO_MODE_AF_PP; GPIO_InitStruct.Speed GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, GPIO_InitStruct); } }这里有几个细节。TIM1 在 F103 上属于高级定时器PWM 输出引脚必须配置为复用推挽PA8 和 PA11 分别对应两路电机使能。GPIO_SPEED_FREQ_LOW 对 10kHz 左右的 PWM 完全够用反而能降低电磁干扰。高级定时器比通用定时器多一个重复计数寄存器 RCR初始化时必须保证 RCR 为 0否则 PWM 输出频率会按 RCR1 倍被拉低这是新手最容易忽视的地方。提示修改 TIM1 的 Prescaler 和 Period 时记得同步修改 Set_Motor_PWM 里的占空比映射上限。很多调车失败案例是用户改了定时器周期但 PWM 限幅值还按旧参数算导致小车只能往一个方向加速。ADC 部分走的是独立多通道连续转换static void MX_ADC1_Init(void) { hadc1.Instance ADC1; hadc1.Init.ScanConvMode ADC_SCAN_ENABLE; hadc1.Init.ContinuousConvMode ENABLE; hadc1.Init.DiscontinuousConvMode DISABLE; hadc1.Init.ExternalTrigConv ADC_SOFTWARE_START; hadc1.Init.DataAlign ADC_DATAALIGN_RIGHT; hadc1.Init.NbrOfConversion 2; HAL_ADC_Init(hadc1); HAL_ADCEx_Calibration_Start(hadc1); // F1 系列必须做校准 }连续扫描模式跑两个通道一个通道接电池电压分压一个通道接电流采样。HAL_ADCEx_Calibration_Start是 F1 系列特有的校准流程漏掉的话 ADC 读值整体偏高低电量保护会提前误触发。读取时机也有讲究最好在 PID 中断之外轮询读取或者用 DMA 自动搬运不要在控制中断里做阻塞式HAL_ADC_PollForConversion那会让控制周期抖动。2.3 外设-功能映射速查表与实际部署要点下面这张表是 B570 上最常见的引脚分配拿到项目代码后先与原理图核对再根据代码注释里的引脚号修正外设功能常见引脚HAL 源文件关键参数TIM1 CH1/CH4左右电机 PWMPA8 / PA11stm32f1xx_hal_tim.cPWM 频率 10kHzRCR0TIM3 编码器左电机速度回读PA6 / PA7stm32f1xx_hal_tim_ex.c编码器模式4 倍频TIM4 编码器右电机速度回读PB6 / PB7stm32f1xx_hal_tim_ex.c编码器模式4 倍频ADC1 CH1/CH2电池电压/电流PA1 / PA2stm32f1xx_hal_adc_ex.c分压比按板子实测校准USART1蓝牙遥控与调试PA9 / PA10stm32f1xx_hal_uart.c波特率 115200I2C1连接 MPU6050PB8 / PB9软件 I2C 或硬件 I2C400kHz对照这张表可以快速定位 main.c 里每段代码的归属。很多课程作业的引脚注释和实际原理图不一致只要 HAL 初始化代码里 GPIO 和 Timer 实例对得上就以代码为准直接用表里的引脚号去对逻辑比反复读注释更可靠。串口初始化也有个高频坑。B570 透传协议常见的是三字节帧帧头 0xFF、方向值、速度值。USART1 中断接收单字节在接收回调里做状态机解析。HAL_UART_Receive_IT必须在每次接收完成后重新调用一次否则只收第一字节程序就停在等待状态。遇到手机蓝牙连不上先拿 USB 转 TTL 接 PA9/PA10 测试确认不是波特率被改过。3. MPU6050 DMP 姿态解算别让原始数据的噪声毁掉控制链路3.1 为什么选 DMP 而不是自己写互补滤波MPU6050 原始输出是三轴加速度计和三轴陀螺仪数据。B570 这种倒立摆应用姿态角度滞后要小噪声要低。互补滤波在静止时表现可以小车一起步、加速、原地转圈加速度计受到运动加速度干扰一阶滤波出来的角度会明显抖动。如果滤波系数调低角度平滑了但滞后变大直立控制会出现低频振荡。DMP 的优势在于传感器内部完成四元数解算和姿态融合MCU 不需要在中断里跑卡尔曼矩阵运算。B570 工程里放 inv_mpu.c 和 inv_mpu_dmp_motion_driver.c正是这个原因。DMP 在 200Hz 采样率下输出稳定的融合角度MCU 只用每 5ms 读一次 FIFO计算负载非常低。对课程设计而言把时间花在调 PID 上而不是反复调滤波截止频率性价比要高得多。要注意的是inv_mpu.c 访问 I2C 底层的接口函数在 mpu_init 之前必须挂好。代码里若是软件模拟 I2CGPIO 翻转延迟直接决定 I2C 时钟频率。我一般把 SCL 高电平时间压在 1.3us 左右对应 400kHz比 100kHz 快四倍DMP 初始化时的 FIFO 清空操作会明显流畅很多。3.2 DMP 初始化链路与中断读取方式下面这段是 B570 工程里标准的 MPU6050 初始化序列void MPU6050_Init(void) { MPU_InitTypeDef MPU_InitStruct; MPU_InitStruct.Accel_FSR MPU_ACCEL_FS_2_G; // ±2g MPU_InitStruct.Gyro_FSR MPU_GYRO_FS_250_DPS; // ±250°/s MPU_InitStruct.DLPF_Mode MPU_DLPF_42HZ; // 低通 42Hz MPU_InitStruct.Sample_Rate 200; // 200Hz MPU_InitStruct.INT_Mode MPU_INT_MODE_RAISING; MPU_InitStruct.INT_Enable MPU_INT_ENABLE; MPU_InitStruct.INT_Type MPU_INT_TYPE_EDGE; mpu_init(MPU_InitStruct); mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_set_sample_rate(200); dmp_load_motion_driver_firmware(); dmp_set_orientation(inv_orientation_matrix_to_scalar( (const signed char *)gyro_orientation)); dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_GYRO_CAL); dmp_set_fifo_rate(100); }初始化里有两个关键取舍。加速度计量程选 ±2g平衡小车倾斜角范围通常不超过 ±20 度±2g 下 1g 对应 16384 LSB分辨率最高。±16g 在这个场景只会白白浪费 ADC 精度。DLPF 设在 42Hz是为了滤掉电机 PWM 带来的高频机械振动同时保留直立控制需要的姿态动态。若 DLPF 设到 10Hz角度会明显滞后小车会前后摆动不停。姿态读取要特别注意 FIFO 积压问题。dmp_set_fifo_rate(100) 让 DMP 以 100Hz 往 FIFO 写数据与控制周期 5ms 并不完全对齐void MPU6050_Task(void) { short quat[4]; long long sensor_timestamp; if (dmp_read_fifo(gyro, accel, quat, sensor_timestamp) 0) { q30_to_float_roll_pitch_yaw(quat, roll, pitch, yaw); } }dmp_read_fifo 返回 0 表示读到了完整一帧。正常情况 FIFO 每秒只积累 100 帧远小于 FIFO 容量不会溢出。但如果 I2C 通信被中断频繁打断FIFO 可能在同一时刻积压多帧。常见做法是在主循环里轮询调用只在读取成功时更新姿态不需要额外做 FIFO 清空。DMP_FEATURE_GYRO_CAL 在上电后头两秒静止放置有效否则零漂会被错误固化导致小车静止时角度缓慢漂移。3.3 四元数转欧拉角的符号约定inv_mpu_dmp_motion_driver.c 输出的四元数是 q30 定点格式每个分量需要除以 16384 转成浮点。转换公式如下void q30_to_float_roll_pitch_yaw(short *q, float *roll, float *pitch, float *yaw) { float q_w (float)q[0] / 16384.0f; float q_x (float)q[1] / 16384.0f; float q_y (float)q[2] / 16384.0f; float q_z (float)q[3] / 16384.0f; *roll atan2f(2.0f * (q_w * q_x q_y * q_z), 1.0f - 2.0f * (q_x * q_x q_y * q_y)) * 57.29578f; *pitch asinf(2.0f * (q_w * q_y - q_z * q_x)) * 57.29578f; *yaw atan2f(2.0f * (q_w * q_z q_x * q_y), 1.0f - 2.0f * (q_y * q_y q_z * q_z)) * 57.29578f; }四元数数组顺序是 q[0]w、q[1]x、q[2]y、q[3]z这个顺序不能搞混。DMP 输出默认按安装时的方向矩阵计算如果调试时发现角度符号反了不用改算法检查 dmp_set_orientation 传入的 gyro_orientation 数组前三个值。我用一张表把常见的符号问题列出来现象可能原因调整位置左倾时 roll 输出为正传感器安装方向反了gyro_orientation 的第一个分量取反pitch 和 roll 互换MPU6050 旋转 90 度安装gyro_orientation 的排列顺序交换静止 30 秒后角度漂移GYRO_CAL 上电时车体在动上电后保证静止 2 秒再直立角度有规律跳动FIFO 读取不及时或 I2C 干扰降低 dmp_set_fifo_rate 到 50Hz另外atan2f 和 asinf 在 F103 上运算是耗时的。如果每个控制周期都调一次5ms 周期内约消耗 200us对实时性有隐患。工程上可以只求 roll 角用于直立控制pitch 以 20ms 周期低速更新这样把姿态解算时间砍掉一半中断里留给 PID 的预算更充裕。4. 串级 PID 控制角度环、速度环与定时器中断的配合4.1 控制量的层次关系平衡小车本质上是一个欠驱动倒立摆只做角度单环控制车身能在平衡角附近稳住但会朝一个方向持续加速直到撞到限位。原因很简单角度回中并不代表位置在原点。B570 的代码里采用串级结构外环速度环输出角度修正量内环角度环根据目标角度和实际角度输出 PWM。具体层级关系是内环角度环输入是目标倾斜角与当前实际倾斜角之差输出为期望角速度外环速度环输入是目标线速度与当前平均轮速之差输出目标倾斜角的修正量最终 PWM 输出 角度环输出 角度目标修正量折算的补偿值角度环要快速度环要慢。控制周期是 5ms速度环每 20ms 才更新一次这样两个环不会互相激发振荡。4.2 定时器中断回调里的串级实现B570 工程通常会在 TIM2 更新中断里调度控制函数。TIM1 负责 PWMTIM3/TIM4 负责编码器回读TIM2 单独作为控制节拍这样各外设不抢占同一个中断源。核心代码结构如下void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { static uint8_t speed_accum 0; static float speed_integral 0.0f; if (htim-Instance TIM2) { float angle_now Get_Angle(); // 当前车体倾角 float error_angle control_target_angle - angle_now; // 内环角度环P 项 D 项D 项直接用陀螺仪角速度 float angle_pwm ANGLE_KP * error_angle; angle_pwm ANGLE_KD * gyro_rate_y; // 外环速度环每 4 个控制周期更新一次 if (speed_accum 4) { speed_accum 0; float speed_now (encoder_left encoder_right) / 2.0f; float speed_error control_target_speed - speed_now; speed_integral speed_error * 0.02f; speed_integral Clamp(speed_integral, -20.0f, 20.0f); control_target_angle ANGLE_STAND SPEED_KP * speed_error SPEED_KI * speed_integral; } // 合成 PWM 并限幅 float motor_pwm angle_pwm (control_target_angle - ANGLE_STAND) * 100.0f; motor_pwm Clamp(motor_pwm, -PWM_MAX, PWM_MAX); Set_Motor_PWM(motor_pwm, motor_pwm); } }这段代码有几个必须注意的地方。角度环的 D 项直接用陀螺仪 y 轴角速度而不是对角度做差分。这样避免了对含有噪声的角度信号求导工程上行得通因为陀螺仪本身就是角速度传感器量纲直接匹配。积分项限幅到 ±20这是防止小车被外力推到一边时积分累积过大恢复后车轮猛冲。Clamp 函数限幅的上下界要根据实测调整我发现 B570 配 7.4V 锂电池时PWM 上限取 700 比较合适对应 10kHz 定时器 70% 占空比。超过这个值电机进入饱和区PID 调节将失效。还要注意编码器回读的符号。左右电机对称安装但编码器计数方向可能相反。如果 speed_now 恒为零或者渐增先交换 TIM3/TIM4 的编码器输入捕获极性配置不要直接调 PID 参数那是白费力气。4.3 PID 参数调试顺序与失败模式判断调参顺序比参数本身更重要。我习惯按下表分阶段进行调试阶段调整参数初始值判断标准只开角度环 PANGLE_KP30手扶车身能感觉到明显回正力加入角度环 DANGLE_KD1.0松开手后回正过程不来回摆动接通速度环 PSPEED_KP0.1小车站立后不持续朝一个方向移动加入速度环 ISPEED_KI0.02长时间站立不向一侧缓慢漂移第一阶段只保留角度环 P用手扶着车体感受推力如果推左倾车身会明显向右顶回来说明这个环的方向和极性是对的。如果扶正后车身反而加速倒向另一侧说明误差符号反了检查 Get_Angle 返回值的正负定义。第二阶段加 D 项角度环响应变快但 KD 太大会产生高频抖动听电机声音就能判断。第三阶段接速度环小车能在原地立住而不是越走越快就算通了。最后加积分项消静态误差。有一个常见误用需要特别指出如果速度环方向接反小车会越走越快直到疯狂旋转。判断方法很简单把小车放在地面目标速度为 0如果实际速度回读为负但速度环输出在增大说明编码器符号反了而不是 PID 调得不对。另外从 5ms 控制周期改成 8ms 时所有积分系数必须按比例放大比如 SPEED_KI 乘以 1.6否则速度环响应会变慢产生低频摆动。5. 在 B570 上部署验证并把代码写成高分布项目5.1 烧录流程与上电自检拿到源码先不要改代码直接编译烧录验证硬件是否正常。STM32F103 用 SWD 四线接口SWCLK、SWDIO、3V3、GND。连接后在 STM32CubeProgrammer 里把 SWD 频率降到 2MHz 以下很多时候烧录失败不是板子坏了是线太长或者电脑 USB 口供电不稳。烧录成功后上电按顺序检查三件事串口助手有没有周期性输出姿态数据电机驱动使能引脚是否被正确拉高MPU6050 的 INT 引脚是否输出边沿信号。5.2 调车现场的验证清单调试时建议配一张实测记录表每调一轮填一次数据检查项通过标准实测记录USART1 姿态输出每 10ms 一行 pitch/roll写入实际值电池电压采集万用表与串口读数误差小于 0.1V写入实际值角度环响应手动倾斜 20 度PWM 输出反向变化记录 PWM 值速度环稳定性静止站立 30 秒无明显位移记录位移距离低电量保护电压低于 7.2V 时 PWM 限幅生效记录阈值这些数据会直接写进课程设计的实验报告里比任何文字描述都有说服力。我见过不少项目代码能跑但文档里拿不出一组实测数据答辩时被问一句「你怎么证明它稳定」就卡住了。5.3 文档编排上的三个加分点核心代码注释密度做到两行代码配一行注释说明变量含义和量纲就够了不要写成小说。文档里重点放三样东西外设引脚速查表、定时器中断时间线、PID 调参记录。调参记录里写明初始值、调整过程、现象变化。例如ANGLE_KP 从 30 加到 50 后车身回正速度加快但出现 2Hz 左右的小幅抖动于是把 ANGLE_KD 从 1.0 提高到 1.8抖动消失。这种记录证明项目是你亲手调过的而不是抄代码。评审老师看到「初始倾角 25 度时 0.8 秒恢复直立」这类实测描述分数天然不会低。本文还有配套的精品资源点击获取
