EEPW论坛

关于开箱图..呃，貌似只留下这么一张照片，不得不说小车做工还是很精良的（点个赞），保护板好像多发了一块

第一次接触平衡小车，首先我打算学一下滤波，网上资料不少，但是都看得晕晕的，唯一一份比较好的（看过之后大概了解卡尔曼了）这里分享给大家平衡小车卡尔曼.doc。大概明白了卡尔曼滤波的5个公式是什么含义

平衡PID的初次调试，在学习PID之前，楼主打算先盲调一下，感受一下俩个系数的作用

首先，保持D不变，将P调高，调高后感觉平衡貌似更好了，但是推一下就会颤动，系数越大，抗干扰能力越差。

那么，将P调小呢...呃，小车倒了....

然后是D，保持P为60，将D调大，调到0.15就开始抖了..

至于往低调呢..貌似0.1是个界限，低于0.1小车就无法直立。

最后P系数调到95，D为0.1，这样平衡效果比较好，抗外力干扰也还行。

这是我对PID的直观感觉，接下来会深入学习。

这是调整参数后的平衡效果

我决定先先来看看PWM产生部分，这是我第一次接触stm32f103，所以打算把对这部分的理解写出来，望大家指出错误。

也就是  void MiniBalance_PWM_Init(u16 arr,u16 psc)  这个函数

首先是对TIM3以及IO口的时钟使能，stm32单片机每次对IO口以及外设操作时都要先使能相应的时钟！

例程使用了TIM3的PWM2模式，stm32的每个通用定时器都可以使出4路PWM。即有4个独立的通道，这里使用了通道3和通道4

查阅手册，有俩个  捕获/比较模式寄存器(TIMx_CCMR1和TIMx_CCMR2)，其中TIMx_CCMR1控制通道1和通道2；TIMx_CCMR2控制通道3和通道4 。

关于PWM的俩个输出模式 ：110：PWM模式1－ 在向上计数时，一旦TIMx_CNTTIMx_CCR1时通道1为无效电平(OC1REF=0)，否则为有效电平(OC1REF=1)。 111：PWM模式2－ 在向上计数时，一旦TIMx_CNTTIMx_CCR1时通道1为有效电平，否则为无效电平。

关于有效电平和无效电平，具体配置在 捕获/比较使能寄存器(TIMx_CCER) 中的 CCxP 位中设置 x 为通道号，CCx通道配置为输出： 0：OCx高电平有效 1：OCx低电平有效  例程中将低电平设为有效，还有貌似例程中应该是PWM模式1才对..

函数入口参数有俩的个，arr自动重装值  psc始时钟分频系数大家可以看这张图来理解，分频器将时钟信号分频，供给计数器使用。

总结一下就是 定时器向上计数值小于 arr ，输出为高电平，计数值大于arr 输出低电平，然后自动重装，输出PWM。

如有错误望大家指出

接下来进行了平衡小车的前进后退实验

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

我将MiniBalence中第18行代码改为如下：

int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
extern int tt;
void TIM1_UP_TIM16_IRQHandler(void)  
{    
 if(TIM1->SR&0X0001)//5ms¶¨Ê±ÖжÏ
 {
  TIM1->SR&=~(1<<0);                                       //===Çå³ý¶¨Ê±Æ÷1Öжϱê־λ
 tt++;
 if(tt<500) 		Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0; 
 if(tt>=500 && tt<1500) Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0; 
 if(tt>=1500 && tt<2000) Flag_Qian=0,Flag_Hou=1,Flag_Left=0,Flag_Right=0;
 if(tt>2000) tt=0;
 readEncoder();                                           //===¶ÁÈ¡±àÂëÆ÷µÄÖµ
  		Led_Flash(400);                                          //===LEDÉÁ˸
  		Get_battery_volt();                                      //===»ñÈ¡µç³Øµçѹ	          
 key(100);                                                //===ɨÃè°´¼ü״̬
  Get_Angle(Way_Angle);                                    //===¸üÐÂ×Ë̬
 			Balance_Pwm =balance(Angle_Balance,Gyro_Balance);        //===ƽºâPID¿ØÖÆ 
 			Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);       //===ËٶȻ·PID¿ØÖÆ
 	    Turn_Pwm    =turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===תÏò»·PID¿ØÖÆ
 		  Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm;                 //===¼ÆËã×óÂÖµç»ú×îÖÕPWM
 	  	Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm;                 //===¼ÆËãÓÒÂÖµç»ú×îÖÕPWM
   		Xianfu_Pwm();                                            //===PWMÏÞ·ù
      if(Turn_Off(Angle_Balance,Voltage)==0)                   //===Èç¹û²»´æÔÚÒì³£
 			Set_Pwm(Moto1,Moto2); 

 //===¸³Öµ¸øPWM¼Ä´æÆ÷     
 }
} 

	



	

视频中我观察到小车由于惯性原因停留过程不是很理想，在后续的学习中，加入编码器的相关参数应该会更好些。

这里有点小问题，我复制的代码注释全部是乱码，不知道怎么解决..

keil复制注释中文显示乱码的解决办法

出现乱码总是让人头痛，百度后得出如下解决办法

这几天学习了一下mpu6050，有一些心得，在这里写出来

我们使用6050的方法其实就是用IIC先向6050寄存器内部写入特定值来初始化他，然后读取6050寄存器的值就得到数据，所以先介绍一下6050的寄存器：

电源管理寄存器1（0X6B）

EVICE_RESE=1，复位MPU6050，复位完成后，自动清零。
SLEEP=1，进入睡眠模式；SLEEP=0，正常工作模式。
TEMP_DIS，用于设置是否使能温度传感器，设置为0，则使能
CLKSEL[2:0]，用于选择系统时钟源
000 内部8M RC晶振
001 PLL，使用X轴陀螺作为参考
010 PLL，使用Y轴陀螺作为参考
011 PLL，使用Z轴陀螺作为参考
100 PLL，使用外部32.768Khz作为参考
101 PLL，使用外部19.2Mhz作为参考
110 保留
111 关闭时钟，保持时序产生电路复位状态

陀螺仪配置寄存器（0X1B）

FS_SEL[1:0]这两个位，用于设置陀螺仪的满量程范围：0，±250°/S；1，±500°/S；2，±1000°/S；3，±2000°/S；

加速度传感器配置寄存器（0X1C)

AFS_SEL[1:0]这两个位，用于设置加速度传感器的满量程范围：0，±2g；1，±4g；2，±8g；3，±16g；

FIFO使能寄存器（0X23）

该寄存器用于控制FIFO使能，在简单读取传感器数据的时候，可以不用FIFO，设置对应位为：0，即可禁止FIFO，设置为1，则使能FIFO。

TEMP_FIFO_EN为内部温度传感器的FIFO使能，其他位都如此

陀螺仪采样率分频寄存器（0X19）

该寄存器用于设置MPU6050的陀螺仪采样频率，计算公式为：
采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)

温度传感器数据输出寄存器（0X41~0X42）

通过读取0X41（高8位）和0X42（低8位）寄存器得到，温度换算公式为：
Temperature = 36.53 + regval/340
其中，Temperature为计算得到的温度值，单位为℃，regval为从0X41和0X42读到的温度传感器值。

配置寄存器（0X1A）

字低通滤波器（DLPF）的设置位，即：DLPF_CFG[2:0]，加速度计和陀螺仪，都是根据这三个位的配置进行过滤的

加速度传感器数据输出寄存器（0X3B~0X40）

加速度传感器数据输出寄存器总共由6个寄存器组成，输出X/Y/Z三个轴的加速度传感器值，高字节在前，低字节在后。

陀螺仪数据输出寄存器（0X43~0X48）

以上就是一些比较重要的寄存器，我们使用6050是，大多是移植来的，论坛里有版主发的移植教程，简单易懂，我把MPU6050.c里的一些函数拿出来分析一下：

首先我们打开头文件，看到的是大块儿的宏定义，这些就是6050寄存器的地址以及相关操作需要写入的值。

配置过程，例如：

void MPU6050_setClockSource(uint8_t source){
    IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);

}
 
就是在向 电源管理寄存器1（0X6B） 写入值，配置6050，接下来的其他配置函数都是如此

/**************************************************************************
函数功能：读取MPU6050内置DMP的姿态信息
入口参数：无
返回  值：无
作    者：平衡小车之家
**************************************************************************/
void Read_DMP(void)
{	
	  unsigned long sensor_timestamp;
		unsigned char more;
		long quat[4];

				dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);		
				if (sensors & INV_WXYZ_QUAT )
				{    
					 q0=quat[0] / q30;
					 q1=quat[1] / q30;
					 q2=quat[2] / q30;
					 q3=quat[3] / q30;
					 Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 	
					 printf("%f\r\n",Pitch);
				}

}

q0=quat[0] / q30; //q30格式转换为浮点数
      q1=quat[1] / q30;
      q2=quat[2] / q30;
      q3=quat[3] / q30; 
      //计算得到俯仰角/横滚角/航向角
      pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角
      roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;//横滚角
      yaw=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;    //航向角

由于平衡小车只用到了俯仰角，所以另外两个省略了。 uat[0]~quat[3]：是MPU6050的DMP解算后的四元数，q30格式。 q30：是一个常量：1073741824，即2的30次方。 57.3：是弧度转换为角度，即180/π，这样结果就是以度（°）为单位的。

这是我的一些心得，也只是一些皮毛

关于6050的FIFO还不是很理解，有待继续学习

这里有一份MPU6050的源码及其说明，大家可以拿去研究

DMP资料.zip

卡尔曼 互补滤波移植实验

楼主手里有一块stm32f407开发板，板上板载了mpu6050，不得不说6050的DMP很方便，但是我觉得滤波算法也是比较重要的，所以试着将平衡车例程中的滤波算法移植到了开发板上，过程如下：

将例程中的filter文件夹复制到工程目录的HANDWARE下，然后添加到keil中，别忘了头文件路径，我注意到例程中没5ms通过中断进行滤波，这应该就对应着滤波算法中的 fioat dt=0.005;（积分时间）所以移植时我们也要注意这一点，每5ms进行一次滤波

下面是timer.c文件中的定时器初始化函数：

void TIM3_Int_Init(u16 arr,u16 psc)
{
	RCC->APB1ENR|=1<<1; //TIM3时钟使能 TIM3->ARR=arr;  	//设定计数器自动重装值 
	TIM3->PSC=psc;  	//预分频器	  
	TIM3->DIER|=1<<0; //允许更新中断 TIM3->CR1|=0x01;    //使能定时器3
  MY_NVIC_Init(1,3,TIM3_IRQn,2);	//抢占1，子优先级3，组2									 
}

(开发板是stm32f407）

然后是倾角读取函数：

void Get_Angle(void)
{ 

	if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
		{ 
			MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//得到加速度传感器数据
			MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//得到陀螺仪数据
			if(gyroy>32768)  gyroy-=65536;     //数据类型转换  也可通过short强制类型转换
			if(gyroz>32768)  gyroz-=65536;     //数据类型转换
	  	if(aacx>32768) aacx-=65536;    //数据类型转换
		  if(aacz>32768) aacz-=65536;    //数据类型转换

	   	aacy=atan2(aacx,aacz)*180/PI;                 //计算与地面的夹角	
		  gyroy=gyroy/16.4;                                    //陀螺仪量程转换	
			if(flag==1)
			Kalman_Filter(aacy,-gyroy);  //卡尔曼滤波	
			else if(flag==2)
				Yijielvbo(aacy,-gyroy);   //一阶互补滤波
	  	}
}

定时中断部分：

void TIM3_IRQHandler(void)
{
	if(TIM3->SR &0x0001)
	{
		TIM3->SR &=~(1<<0); Get_Angle(); } } 

主函数：

u8 flag=1;     //1:卡尔曼滤波       2：互补滤波

int main(void)
{       
	Stm32_Clock_Init(336,8,2,7);
	delay_init(168);			//延时初始化  
	uart_init(84,115200);		//初始化串口波特率为500000
	MPU_Init();					//初始化MPU6050
	while(mpu_dmp_init())
	{

		delay_ms(200);
 		delay_ms(200);
	}
		TIM3_Int_Init(50-1,8400-1);
 	while(1)
	{	
		printf("%5f,%5f\r\n",pitch,angle);
	} 	
}

今天调试是由于还不太了解示波上位机的协议，所以用了串口打印的方法调试，pitch为DMP读出数据，作为参考，angle为滤波结果，波形上位机以后再补图。

移植完成后滤波效果还不错，就是收敛有些慢，参数有待调整。

今天对上次滤波的参数做了调整，主要是调整了积分时间，得到了滤波效果不错的波形，本次试验使用了匿名四轴的上位机，下面会附上

这是得到的波形（蓝色为6050DMP所得，作为参考）

可以看出卡尔曼滤波得到的结果还是有些延迟，在峰值处比较明显

下面这张图是剧烈摇晃开发板是得到的波形，卡尔曼与DMP出现了明显的偏差：

下面附上程序（stm32f407）

void usart1_send_char(u8 c)
{
	while((USART1->SR&0X40)==0);//等待上一次发送完毕   
	USART1->DR=c;   	
} 
//传送数据给匿名四轴上位机软件(V2.6版本)
//fun:功能字. 0XA0~0XAF
//data:数据缓存区,最多28字节!!
//len:data区有效数据个数
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
	u8 send_buf[32];
	u8 i;
	if(len>28)return;	//最多28字节数据 
	send_buf[len+3]=0;	//校验数置零
	send_buf[0]=0X88;	//帧头
	send_buf[1]=fun;	//功能字
	send_buf[2]=len;	//数据长度
	for(i=0;i<len;i++)send_buf[3+i]=data[i];			//复制数据
	for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i];	//计算校验和	
	for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]);	//发送数据到串口1 
}
//发送加速度传感器数据和陀螺仪数据
//aacx,aacy,aacz:x,y,z三个方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
void mpu6050_send_data(short aacx,short aacy)
{
	u8 tbuf[4]; 
	tbuf[0]=(aacx>>8)&0XFF;
	tbuf[1]=aacx&0XFF;
	tbuf[2]=(aacy>>8)&0XFF;
	tbuf[3]=aacy&0XFF;
	usart1_niming_report(0XA1,tbuf,4);//自定义帧,0XA1
}

int main(void)
{       
	Stm32_Clock_Init(336,8,2,7);
	delay_init(168);			//延时初始化  
	uart_init(84,500000);		//初始化串口波特率为500000
	MPU_Init();					//初始化MPU6050
	while(mpu_dmp_init())
	{

		delay_ms(200);
 		delay_ms(200);
	}
		TIM3_Int_Init(85-1,8400-1);
 	while(1)
	{	
		mpu6050_send_data(pitch,-angle);//用自定义帧发送加速度和陀螺仪原始数据
	} 	
}

程序中可以看出积分时间为8.5ms

大家帮忙看看，不知道这样的滤波效果能不能用来做平衡车

ANO_Tech匿名四轴上位机_V2.6 (1).zip     弹出的小篮框直接无视Alt+F4，之后可能会提示下载出错，无视，按F12弹出使用帮助