EEPW论坛

6、程序设计

    程序思路如下：STM32通过读取传感器数据，做滤波和姿态计算，然后通过串级PID进行控制；同时接收遥控指令，做出前后左右以及旋转的动作。下面开始分步介绍：

1)MPU6050采用I2C总线驱动，楼主使用模拟I2C，速度很快，100多个us就能读完传感器数据，然后进行滤波+姿态解算，得到小车的角度信息。

    数据处理和PID控制就是自平衡小车的核心了。我们对加速度计做低通滤波，对陀螺仪做滑动滤波，过滤大部分由震动引起的数据噪声。 然后用四元数来计算姿态，最后转换成角度（欧拉角），就能用于控制了。初次接触MPU6050的网友可能对数据解算比较陌生，建议看一下下面这个精品贴： 【讨论帖】姿态解算相关知识储备

关于MPU6050的驱动：： stm32f103运行mpu6050源码

为什么选择6050呢，因为它内部集成了一个硬件算法，能够直接输出表示姿态的四元数，我们只需要把这个四元数用公式转化成角度（欧拉角），就能用于控制了，是不是很方便呢！前期调过姿态解算，后期再慢慢研究姿态解算的问题。

公式如下：

Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

实际上我们用到一个角度即可

关于MPU6050的DMP功能，请看：传感器驱动实验（DMP测试，串口回传欧拉角及四元数，附视频，工程文件）(欧拉角万向锁视频)

2）串级PID相关介绍：串级PID----代码+调参

3）Nrf2401通讯实验：NRF无线通讯实验（附视频，工程文件）

4)电机控制，当然要搞清楚这个电机驱动怎么用啦，我们一起来看看：

TB6612的的用法：

TB6612是双驱动，也就是可以驱动两个电机

下面分别是控制两个电机的IO口

STBY口接单片机的IO口清零电机全部停止，置1通过AIN1 AIN2，BIN1，BIN2 来控制正反转

VM  接12V以内电源

VCC 接5V电源

GND 就不多说了啊

驱动1路

PWMA      接单片机的PWM口  

真值表：

AIN1 0      0      1

AIN2 0      1      0

     停止 正传   反转

A01  

AO2   接电机1的两个脚

驱动2路

PWMB     接单片机的PWM口

真值表：

BIN1 0      0      1

BIN2 0      1      0

    停止 正传   反转

B01

BO2   接电机2的两个脚

    很容易写出电机驱动，包括转速控制和前进后退控制

/******************************************************************************
函数原型：	void Motor_Out(int16_t duty1,int16_t duty2)
功    能：	电机驱动
*******************************************************************************/ 
void Motor_Out(int16_t duty1,int16_t duty2)
{
	if(duty1>1000)	duty1=1000;
	if(duty1<0)		duty1=0;
	if(duty2>1000)	duty2=1000;
	if(duty2<0)		duty2=0;
	
	TIM2->CCR1 = duty1;
	TIM2->CCR2 = duty2;
}

/******************************************************************************
函数原型：	void Forward(void)
功    能：	前进
*******************************************************************************/ 
void Forward(void)
{
	STBY_H;
	AIN1_L;AIN2_H;
	BIN1_H;BIN2_L;
}

/******************************************************************************
函数原型：	void Back(void)
功    能：	后退
*******************************************************************************/ 
void Back(void)
{
	STBY_H;
	AIN1_H;AIN2_L;
	BIN1_L;BIN2_H;	
}

/******************************************************************************
函数原型：	void Stop(void)
功    能：	停止
*******************************************************************************/ 
void Stop(void)
{
	STBY_L;
	AIN1_L;AIN2_L;
	BIN1_L;BIN2_L;	
}

7、调试流程

    程序对应了一个MWC开源飞控的上位机，简单实用，主要调试工作通过上位机即可，下面开始讲解：

1，把遥控器的串口1链接到电脑上，小车和遥控器通过2401进行半双工无线数据。传输打开上位机，即MultiWiiConf文件夹下的MultiWiiConf.exe。下载地址：MultiWiiConf.rar

要想运行MultiWiiConf.exe，得先安装JAVA虚拟机。Multiwii 的GUI程序需要JAVA虚拟机的支持，需下载安装：http://java.com/zh_CN/download/manual.jsp?locale=zh_CN
（如果你的电脑是win7 64位的系统，安装了JAVA虚拟机后GUI仍然运行不了，那你就需要到C:\Program Files\Java\jre7\bin\找到并复制javaw.exe，然后粘贴到C:\Windows\SysWOW64\这个文件夹下面，再重新运行GUI即可）成功运行则出现如下界面，会显示出PID参数，遥控数据，姿态，传感器数据等等，同时可以通过上位机进行PID参数设置。

2，加速度计和陀螺仪（MPU6050）由于安放的位置，是有零偏的，带着这个零偏去做姿态解算，就会造成解算误差，因此要去除这个零偏。通过上位机可以处理零偏问题。

A、校准加速度计

具体操作：将小车贴墙放置，这样一来就保证了小车是竖直向上的。遥控器连接电脑，运行GUI，点击CALIB_ACC，你会发现ACC的pitch、roll、Z三轴输出值会变成0,0,512左右的值，则表明校准完成。

B、校准陀螺

具体操作：在校准了加速度计基础上，点击CALIB_MAG，GYRO的pitch、roll、Z三轴输出值会变成0,0,0左右的值，则表明校准完成。

C、调整PID参数

具体操作：将鼠标定位在待调整的PID参数上，按住鼠标左键左右移动鼠标即可调整该数值，很为奇特。
ROLL对应的PID参数是外环参数，PITCH对应的PID参数是内环参数。慢慢调参数即可。调参方法详见：串级PID----代码+调参

8、视频展示

说明：同一个参数，针对    1，没有额外负载；2，放万用表；3，放橘子；都能很好平衡。

    小车的运动控制，用到了前面提到的遥控器。

    有密码的话，请输入“183”

视频地址：http://player.youku.com/player.php/sid/XOTEwNzI4NTA4/v.swf
 

9、个人总结

    首先，此次自平衡小车制作，耗时较短，还有N多功能没加上。近期有很多事，考虑四月份后加上新功能，敬请期待。参数多试几次就出来了，效果还可以，有时间再调调。

     有以下几点收获也体会，跟大家分享下：

    1，楼主用的是11.1V航模电池，放电倍数20C。经过稳压后给电机驱动和STM32+MPU6050供电。不清楚是否较差的电池会对传感器数据造成影响。反正这个航模电池没问题。

    2，角速度要滤波，实际调试的时候，看到上位机的角速度震动很大，这样子就给控制造成了很大震荡，经过滤波后，控制效果较好。

    3，有一个上位机真心方便，点几下上位机，参数就设置好了，功能强大，还能分析传感器数据。

    4，小车的电池要固定好，不能晃动，最好在中心，这样调试比较方便容易。

    5，小车十个很有意思的项目，有坛友回复说在小车上装个摄像头，这想法很棒，后期装一个试试，看看不同视角能拍到啥有意思的~

小车一买，程序一写，参数一挑，OK啦快来吧