【Nucleo】基于stm32F072的双轮自平衡车（减速电机PID/MPU6050）
本帖最后由 xyc2690 于 2015-2-9 00:43 编辑平衡车初步完成~~https://v.youku.com/v_show/id_XODgyOTI4OTU2.html
硬件资料：
控制板： NucleoF072RB
传感器： MPU6050
电源： 聚合物锂电池12V 4000mAh
电机： 直流减速电机+编码器
电机驱动： TB6612
DC-DC降压：LM2596S 、AMS1117-5
两轮自平衡控制系统是一种两轮左右平行布置的,像传统的倒立摆一样,本身是一个自然不稳定体,必须施加强有力的控制手段才能使之稳定。
平衡车工作原理是系统以姿态传感器(陀螺仪、加速度计)来监测车身所处的俯仰状态和状态变化率,通过高速微控制器计算出适当数据和指令后,驱动电动机产生前进或后退的加速度来达到车体前后平衡的效果。假设我们以车上重心纵轴作为参考线。当这条轴往前倾斜时，车身内的内置电动马达会产生往前的力量，一方面平衡车往前倾倒的扭矩，一方面产生让车辆前进的加速度，相反的，当陀螺仪发现重心往后倾时，也会产生向后的力量达到平衡效果。
控制器包括车体状态运算和平衡控制运算2个部分。
车体状态运算主要是将传感器测量的数据加以融合得出车体倾斜角度值、倾斜角速度值以及行车速度等。
平衡控制运算根据车体状态数据，计算保持平衡需要的行车速度和加速度，或者转弯所需要的左右电机速度变化值，向电机控制驱动模块发送控制指令。
运算模块相当于两轮自平衡电动车的大脑，它主要负责的工作是：控制电机的起停，向控制模块发出加速、减速、电机正反转和制动等速度控制信号，接收电机光电信号进行车速度计算。
加速度传感器能过输出以其芯片为中心的三轴加速度，通过这三个轴的重力加速度便可以计算出芯片的倾角，即车体的倾角。该方案的优点是重力加速度的静态性能很好，在车体静态下能测出准确稳定的倾角，而在动态下，三轴加速度各轴会受到其它加速度的影响，导致其数据并不稳定可靠。 陀螺仪传感器能输出围绕以芯片为中心的三个轴的角速度，通过读角速度的积分，即可得出倾角。优点是陀螺仪的动态性能很好，在动态下测出的角速度没有太多的混杂成分，缺点是陀螺仪具有静态漂移，即静态下，陀螺仪仍然会输出数值，而积分却一直在进行，因此静态时，测出来的角度并不是0°。 加速度传感器与陀螺仪传感器结合，通过融合算法，提取出加速度传感器的静态效果和陀螺仪的动态效果。优点是能测出准确稳定的倾角，但融合算法比较复杂。
芯片选择：芯片：MPU-6050供电电源：3-5v（内部低压差稳压）
通信方式：标准IIC通信协议
芯片内置16bit AD转换器,16位数据输出
陀螺仪范围：±250 500 1000 2000°/s
加速度范围：±2 ±4 ±8 ±16g
直接读取MPU6050的原始数据，可以得到xyz3轴加速度和3轴角加速度的值。
两种方案，一是自己对数据处理，把加速度计和角加速度的值进行互补滤波，然后计算倾角。
二是使用芯片内部的DMP，获取融合好的四元数，然后转换成欧拉角。
六轴MPU6050官方DMP移植到F072：详见帖子尾部链接。
********************P I D******************************
PID控制器的输入输出关系：
其中e(t)是实际值与设定值的偏差。
比如这里我们假设车子直立的角度为0（设定值），实际车子向前倾斜了10°（实际值），偏差就是10°
Kp Ki Kd 是系数。
看起来容易吧，实际上参数的整定是很麻烦的过程，要有耐心。
*************************************************************
********************调试******意外******************************
活动买的stm32f072 nucleo板，本来做平衡车的的，调试电机时，给接外部5v电源，错接到PD2脚，然后不幸听到了微弱的嗞嗞声，板子就挂掉了，现在一通电就开始发热发烫，但是程序还能正常跑。。。看来今天晚上可以休息休息洗洗睡了:'(接上USB，stlink指示灯红灯闪烁，说明有电压，没信号。stlink和板子电源不通。
JP5跳帽换到E5V，外部5V供电，U4输出2V左右电压。
给板子接3.3V电源，主芯片发烫，程序能正常跑。。。
多谢版主重新发了一块。。
我的电源分析：
https://www.stmcu.org.cn/module/forum/thread-599203-1-1.html
下面说说调试中遇到的问题和心得吧，。。
*********************************************************************
SWDIO被占用的问题
PA13 和 PA14好像是，如果被 配置成GPIO口，就无法连接下载了，
解决方法:
1.Flash Loader Demonstrator串口擦除程序
2.连上电脑有U盘符，直接用mbed生成的bin文件下载正常配置的程序覆盖。
HAL库的使用
不太习惯用HAL库，简单的串口通信做的好复杂。。。中断、定时器看了好久。
不过使用STM32CubeMX对工程进行初始化配置很方便，层次很清楚，
阅读固件库对我们这种新手养成良好的编程习惯帮助很大。
中断用回调函数实现、串口通信数据接收要建立缓冲区，有必要最好封装成数据帧。
******************************************************************************
希望大家爱护板子。。不要像我一样。。。
让我去好好反省一会儿。。。
******************************************************************************
要抓紧时间了，一月都过去一半了！！
给自己写个调试进程：
2015.01.15
调试完成减速直流电机的驱动
模拟IIC对着示波器调了好久，终于OK了。用的内部上拉电阻，阻值太大，上升沿不太好。终于读到取MPU6050 寄存器的3轴加速度和3轴角速度还有温度了。
**************************************************************************
2015.01.23
放假了回家好好调小车了！！一个礼拜不知道来不来得及~
早上调好了PWM，用的STM32cubeMX自动配置，真是傻瓜操作了= =
下午继续做，晚上回来汇报。
**************************************************************************
整合了一下以前的程序，用mpu6050原始数据的变化，通过TIM3的PWM控制电机通过。
关于6050的教程：第一个是野火的，讲解怎么用，第二个就是原始数据的含义以及一些处理方法。
**************************************************************************
2015.01.28
原始数据处理确实麻烦，互补滤波出来数据漂漂漂。。还是移植官方的DMP吧。
**************************************************************************
2015.01.29
折腾了3天终于把DMP移植好了！本来觉得好累，不过看到出来的Pitch 和Roll 那么稳定，哇咔咔！！
FIFO溢出问题没解决，不过被我机智地注释掉了~~
**************************************************************************
2015.01.30
月的最后两天了，平衡车已经能站起来了，暂时只加上了简单的PID参数慢慢调整吧，现在看起来效果还不错的。推都不倒~
***********************************************************************2015.02.02
角度闭环搞定了，下个目标：速度环。后期考虑加上无线通信模块，载人什么的电机肯定不行了，要不装个机械臂。。
***********************************************************************
先传了个短的10秒视频：
https://v.youku.com/v_show/id_XODgyOTI4OTU2.html
2015.02.08
前几天不在家，下午刚回来，编码器调试好了，开始正式调节PID的速度环。
简单说一下这里我对PID的理解，如有错误的地方请大家指出来哈！
现在的被控量，也就是这里电机上电压的 占空比=Kp*角度+Kd*角加速度+K(speed)*速度+Ki(x)*位移
之前已经通过角度（比例P）和角加速度（微分D），让小车能够在0°左右平衡站立不倒了，现在要通过安装在电机上的光栅编码器，读出转过的角度，转化成小车的速度。我的编码器是AB相的，相位相差90°，AB相先后表示正转和反转。
PD调节小车站立，可是如果用手去推，小车就会向一边加速倒去，这时原先的Kp和Kd参数产生的调节力度就不足以使它保持平衡。现在获得了车的速度，那么就可以在控制方程里加上K(speed),同样做为一个比例系数，反馈到处理器，来增大调节力度。进一步的，我们把速度进行积分，就可以得到车的位移，从而又能产生一个系数Ki(x)，让小车位置保持在原来的位置。
这样两个控制环：角度环和速度环，其中包括了两个比例环节和一个微分环节和一个积分环节，最终就可以实现平衡车的基本功能了。
前进、后退、转弯、旋转等功能的实现也依赖于此，比如说前进的实现，只要在平衡的基础上，适当增加PWM的输出就可以了。
说起来比较简单，调试起来就很麻烦了，先从比例环节开始调，让系统开始振荡，然后调节积分环节。。。有时间上位机写个画图的软件，PID调节就方便多了。
一直调到电池没有电了，本来准备通宵吧参数都做完了，现在可以洗洗睡了。。。等这两天参数调好了就让平衡车跑起来！！
***********************************************************************
***********************************************************************
六轴MPU6050官方DMP移植到F072：
https://www.stmcu.org.cn/module/forum/thread-599730-1-1.html
***********************************************************************
本帖最后由 xyc2690 于 2015-1-29 16:18 编辑
DMP读取四元数，转化成欧拉角~~
烧多了经验就来了，哈哈 简单 去淘宝或者代理商那 弄几篇072RBT6就解决了 harvardx 发表于 2015-1-10 17:18
简单 去淘宝或者代理商那 弄几篇072RBT6就解决了
主要是某觉得电源管理芯片也可能挂掉了。。:'( 本帖最后由 harvardx 于 2015-1-10 18:04 编辑
那个应该不怕, 主要是看看U4 那个要是坏掉了 焊接有麻烦. 先把电源部分检查下看有没问题，芯片未必有问题 770781327 发表于 2015-1-10 19:34
先把电源部分检查下看有没问题，芯片未必有问题
检查过了芯片肯定是坏了，电表测了下，GND 和VDD短路了。
电机和板子没有光耦隔离，就知道会这样。。。
芯片这边估计还买不到，淘宝10块钱一片，快递10块钱，伤心，，，
U4那块电源芯片很难焊，希望是好的吧，，， 平衡车是个好东西啊，又是个考算法的题目 xyc2690 发表于 2015-1-10 19:47
检查过了芯片肯定是坏了，电表测了下，GND 和VDD短路了。
电机和板子没有光耦隔离，就知道会这样。。。
...
恩，芯片智能买了，u4确实不大好焊，可以考虑这部分自己搭电路替换 GND和VCC短路不一定就说明芯片坏了 也有可能是电源保护了 仔细检测一下