【从零开始的两轮自平衡车程序设计】03——KEIL程序编写

1.程序生成
接上一篇，配置完成后，生成KEIL程序。

2.初始化
（1）MPU6050初始化

需要设置时钟源，量程，然后使能6050，量程可以自己调整，是不是最大量程没有关系，具体寄存器的值可以对照6050的手册，只要使用I2C读写函数往对应地址写入数据即可。
关于i2c这块，不清楚是否是103的i2c有问题还是怎么了，下几次程序后，有可能出现初始化不成功的状态，必须断电重启，这个问题我就没有深究了。
（2）初始化DMP
DMP是6050内置的计算程序，可以帮你计算出姿态角。如果对于导航方面有兴趣，可以直接采集加速度和角速度，可以有三种处理：
a.直接计算俯仰角：利用加速度与俯仰角的关系，求个atan2，但注意单位是弧度，这种算法会有一定噪声，不过应该不影响控制
b.互补滤波：在a的基础上加上对陀螺数据的应用，俯仰角的变化速率是Gx，那么k时刻的俯仰角可以认为是k-1时刻的俯仰角加上Gx*T，但是陀螺也有缺陷，它的数据长期会漂移，就是它数据有零偏，这个零偏是随机游走的，而且积分后，这个零偏不像白噪声可以变成0，它会一直增大，所以不能完全使用陀螺数据。因此，把陀螺算出来的俯仰角和加表算出来的俯仰角做组合，就相当于各信一点。
c.卡尔曼滤波：这是从互补滤波的进一步，两个信息来源要组合，到底该多少系数合适，卡尔曼就是解决了这个问题，按照信息的方差的反比来设计比例，方差小的系数大，但是方差的多少是要由你来设定的，Q和R矩阵的大小。所以其实不是一个容易的设定，而且计算也麻烦些，要求矩阵的逆，也要保证P阵是正定的，在这里看不出太大效果，建议仿真的时候可以去学习。关于卡尔曼的理解，可以看我之前的文章，从最小二乘去理解。

这里我使用DMP直接解算俯仰角。
（3）其余功能的开启

开启ADC校准以及采集，PWM输出，编码器使能，开启外部中断。这都是用HAL库函数一句实现，这就是使用HAL库的优势
（4）中断函数
本工程里，主函数没有任何功能，全在中断中实现，虽然F103计算能力差，但是中断时间5ms足够长，所以是足够使用。
中断是程序里最重要的部分。
a.读取姿态以及角速度

对于转向环，我这里没有做，所以也就没有使用航向角以及对应的角速度。
b.读取车轮转速

c.控制算法计算以及转化为对应PWM的占空比，控制算法在下一篇章节展开。


我这里使用了一个IS_start变量，也就是上电延时后一段时间才开始控制。因为我发现采集到的6050俯仰角收敛很慢，输出后还会变一个台阶，很影响控制，我不知道是这个芯片的原因还是因为啥，我折腾了很久只能延时，有了解的人告诉我下要怎么处理。

d.串口数据填充发送以及电压采集

数据填充的格式根据你用的上位机决定，有很多开源的画图上位机，比如VOFA,我用的是纸飞机。

对于电压采集，我这里做了个滤波，一开始想做电压补偿，其实没啥必要，不过作为滤波器离散化的学习也可以。这里必须把计算放在中断保证时序，离散化的运算必须保证运算周期和离散周期一致，不然频率特性不一样。关于滤波器的学习心得，后续有机会再发个文章。

e.周期性点灯

LED指示运行状态，这还是很有必要的。

综上，就是自平衡小车的程序部分了，所有无聊的部分都说完了，接下来就是控制部分。