stm32单片机自制平衡小车【平衡部分】

前言

       1、 该文章仅作为分享以及自己学习单片机过程的记录；

       2、该项目是在学习完b站up主江协科技（原江科大自化协）的stm32系列入门视频后制作的小项目，其中大部分代码是参考其提供的代码，希望能给同样正在学习单片机的同学一些启示；

       3、由于本人刚接触单片机不久能力不足，该文可能有一些错误，欢迎评论指正。

一、准备

硬件方面：

        一块stm32f103c8t6开发板；

        一块mpu6050陀螺仪；

        一块tb6612电机驱动模块；

        一块电池（本人使用的是4.8v，应以实际需要为准）；

        一块0.96寸iic协议的oled屏；

        一个平衡小车底座；

        一块面包板（用于验证电路）；

        焊接工具、排针排母等。

软件方面：

        keil5软件；

        一个绘制pcb的软件（嘉立创、AD等）。

二、程序思路

大致框图：

注解：

由于该平衡小车只完成了小车自平衡部分，因此总体思路较为简单。在while语句中，不断获取陀螺仪的数据，并计算出最终的角度，然后输出经过速度环和直立环相加得到的数值并赋值到对应PWM的CCR参数中。电机上的霍尔编码器数据传回单片机并算出当前小车的速度后，重新计算出新的值并赋给CCR。最终一直循环即可让其保持平衡。

三、主要代码

主函数：

        先是各种定义变量以及初始化，看源码即可。

while (1)
	{
		Get_Angle(&AccX,&AccY,&AccZ,&GyroX,&GyroY,&GyroZ,&Accel_Angle_x,&Accel_Angle_y);
		S = Balance(Pitch)-Velocity(S1,S2);
		Motor_SetSpeed1(S); 
		Motor_SetSpeed2(S); 
	}

        while语句中的Get_Angle(...）函数是获取当前的各种最终角度，其中已计算完成了卡尔曼滤波部分。Balance()函数是直立环，Velocity()是速度环，S变量的值最终输出的是一个int型变量，Motor_SetSpeedx()函数用于将S的值赋值为PWM中的CCR值，用于修改输出的占空比，从而控制电机旋转速度。

获取角度值

{
    MPU6050_GetData(AccX, AccY, AccZ, GyroX, GyroY, GyroZ); //获取陀螺仪原始数据
	*Accel_Angle_x=atan2(*AccY,90-*AccZ)*180/PI;     //求倾斜角，单位为度
	*Accel_Angle_y=atan2(*AccX,*AccZ)*180/PI;     //求倾斜角，单位为度
	*GyroX=(*GyroX/16.4)+2;                       //+2是为了保持陀螺仪在水平的位置为0
	*GyroY=(*GyroY/16.4)+1;          
	float GyroXf = (float)*GyroX;                
	float Accel_Angle_xf = (float)*Accel_Angle_x;    //进制转换
	Pitch=-Kalman_Filter_x(Accel_Angle_xf,-GyroXf);  //使用卡尔曼滤波求出最终角度
}

        在这个函数中，获取陀螺仪原始数据实际上是对其寄存器进行读写，详情可观看up主江协科技的stm32教程中的MPU6050那一节，这里不再赘述。求倾斜角时候由于我的小车的电路板是反装在车架下的，如下图。因此需要90-*AccZ。卡尔曼滤波部分我参考了网上其他的一些开源项目。感兴趣的可以自行下载源码研究。

直立环和速度环

直立环：

balance=-Balance_Kp/100*Angle_bias-Gyro_bias*Balance_Kd/100;

        其公式为直立环的重点，通过修改Balance_Kp和Balance_Kd的值，再将上面获取到的角度值代入这个公式，输出值即实际上赋给CCR的值。当只有直立环且kp和kd值调好后，小车应该出现较为稳定的姿态，能保持一定的平衡但维持不久。

速度环：

velocity=-Encoder_bias*Velocity_Kp/100-Encoder_Integral*Velocity_Ki/100;

        其大体上与直立环类似，通过修改Velocity_Kp和Velocity_Ki的值，最终速度环的值与直立环的值相加并输出驱动电机。当小车加入速度环之后，小车正常的现象应为小范围的摇摆，但此时小车已经能够保持平衡且能维持很久。

四、调试

（这部分强烈建议上网找调参教程，下面是我个人参考视频后得出的一些结论）

        调试直立环一般从kp值开始，需要调整到小车有大幅的低频震荡为止。确定kp值的正负时，需要满足小车向前倾斜时，车向前进，当向后倾斜时候，小车往后退。当调试到最后感觉小车反应有些慢，应考虑是否有其他进程占用资源了，类似oled屏幕显示刷新。获取角度的间隔太长时，小车的反应自然慢了；

        确定直立环的kd值的正负时，可以将kp值先注释掉，先给kd值定大一些，kd极性正确的现象应该是小车向前快速倾斜时轮子需要向前转动，反之也一样。将kd值和kp值调试调到小车出现高频的震荡时，同时将kd和kp值同×0.6，即可出现较为稳定的姿态，虽维持不久但能保持一定的平衡。

        速度环的kp值是ki值的200倍，因此只需调整kp值。确定kp的正负时，可将直立环去掉并将其值定大一些，现象是将轮子向前拨动时，车轮会加速。在直立环的基础上添加速度环后，现象是小车会在一定范围内摆动，需要更大的耐心并慢慢调试即可消除摇摆。

源码地址

链接：https://gitee.com/cylyxx/balancecar.git