EEPW论坛

5、搭建平台

基本零件如下（特意摆了个造型，看看像什么）：

下面开始组装+焊接：

1）先把电机支架跟底盘支架固定起来，如图（简单，拧螺丝即可）：

2）把底盘安装到底盘支架上如图（简单，拧螺丝即可）：

从背面看起来是这样子的：

3）把电机装到电机支架上，同样拧螺丝，注意电机和编码器接口的位置，这么安装，小车倒了后不会装到接口。什么？你说你的小车永远不倒。。。那你随意~

从侧面看起来是这样的：

4）把联轴器装到电机输出轴上（简单，拧螺丝即可）：

 5）装上轮子（简单，拧螺丝即可）：

装好后，从侧面看起来是这样子，有点意思了：

6）开始涉及电路板了，下面是把5V稳压、40P锁紧座、MPU6050、电机驱动、Nrf2401等焊接在一个6cm*8cm的洞洞板上：

把各个模块都装上就是这样：

背面电路走线

7）然后把控制板装到小车上，有模有样啦：

另一面是这样子的：

上面看起来，是这样子的：

8）加上铜柱和上板，有没有一点“高”大上了：

确实有点“高”，“高“主要是是为了下载程序时插拔仿真器方便些~

9）加上电池，齐活了：

给我的格氏电池一个特写：

好啦，至此硬件部分的工作基本完成，小车平台的搭建宣告结束~

接下来是进入软件部分，振奋人心的时刻很快就要到来

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