小车上个月就回来了，期末考试忙着复习，前几天又刚刚搬完宿舍（累死），现在终于有时间玩玩小车啦，嘿嘿。故开此贴，与大家交流
目录
共10条
1/1 1 跳转至页
第一次接触平衡小车，首先我打算学一下滤波，网上资料不少，但是都看得晕晕的，唯一一份比较好的（看过之后大概了解卡尔曼了）这里分享给大家平衡小车卡尔曼.doc。大概明白了卡尔曼滤波的5个公式是什么含义
平衡PID的初次调试，在学习PID之前，楼主打算先盲调一下，感受一下俩个系数的作用
首先，保持D不变，将P调高，调高后感觉平衡貌似更好了，但是推一下就会颤动，系数越大，抗干扰能力越差。
那么，将P调小呢...呃，小车倒了....
然后是D，保持P为60，将D调大，调到0.15就开始抖了..
至于往低调呢..貌似0.1是个界限，低于0.1小车就无法直立。
最后P系数调到95，D为0.1，这样平衡效果比较好，抗外力干扰也还行。
这是我对PID的直观感觉，接下来会深入学习。
这是调整参数后的平衡效果
视频地址：http://player.youku.com/player.php/sid/XMTI4NTk5MDI2NA==/v.swf
我决定先先来看看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¼Ä´æÆ÷ } }
视频中我观察到小车由于惯性原因停留过程不是很理想，在后续的学习中，加入编码器的相关参数应该会更好些。
这里有点小问题，我复制的代码注释全部是乱码，不知道怎么解决..
这几天学习了一下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);
}
}
中，就是从6050相关寄存器读出值，这里要说一下：MPU6050 DMP输出的是姿态解算后的四元数，采用q30格式，也就是放大了2的30次方，我们要得到欧拉角，就得做一个转换，代码如下：
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的源码及其说明，大家可以拿去研究
卡尔曼 互补滤波移植实验
楼主手里有一块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弹出使用帮助
共10条
1/1 1 跳转至页
回复
| 有奖活动 | |
|---|---|
| 【有奖活动】分享技术经验，兑换京东卡 | |
| 话不多说，快进群！ | |
| 请大声喊出：我要开发板！ | |
| 【有奖活动】EEPW网站征稿正在进行时，欢迎踊跃投稿啦 | |
| 奖！发布技术笔记，技术评测贴换取您心仪的礼品 | |
| 打赏了！打赏了！打赏了！ | |
| 打赏帖 | |
|---|---|
| 与电子爱好者谈读图二被打赏50分 | |
| 【FRDM-MCXN947评测】Core1适配运行FreeRtos被打赏50分 | |
| 【FRDM-MCXN947评测】双核调试被打赏50分 | |
| 【CPKCORRA8D1B评测】---移植CoreMark被打赏50分 | |
| 【CPKCORRA8D1B评测】---打开硬件定时器被打赏50分 | |
| 【FRDM-MCXA156评测】4、CAN loopback模式测试被打赏50分 | |
| 【CPKcorRA8D1评测】--搭建初始环境被打赏50分 | |
| 【FRDM-MCXA156评测】3、使用FlexIO模拟UART被打赏50分 | |
| 【FRDM-MCXA156评测】2、rt-thread MCXA156 BSP制作被打赏50分 | |
| 【FRDM-MCXN947评测】核间通信MUTEX被打赏50分 | |
STM32
MCU
通讯及无线技术
物联网技术
电子DIY
板卡试用
基础知识
软件与操作系统
我爱生活
小e食堂