mpu6050卡尔曼滤波输出姿态角程序
/********************(C)COPYRIGHT2012WildFireTeam**************************
*文件名:main.c
*描述:I2CEEPROM(AT24C02)测试,测试信息通过USART1打印在电脑的超级终端。
*实验平台:野火STM32开发板
*库版本:ST3.0.0
*作者:wildfireteam
**********************************************************************************/
#include“stm32f10x.h”
#include“I2C_MPU6050.h”
#include“usart1.h”
#include“delay.h”
#include“filter.h”
#include“math.h”
#include“misc.h”
#include“TIMX.h”
#defineAIN2PBout(15)
#defineAIN1PBout(14)
#defineBIN1PBout(13)
#defineBIN2PBout(12)
#defineBITBAND(addr,bitnum)((addr&0xF0000000)+0x2000000+((addr&0xFFFFF)《《5)+(bitnum《《2))
#defineMEM_ADDR(addr)*((volatileunsignedlong*)(addr))
#defineBIT_ADDR(addr,bitnum)MEM_ADDR(BITBAND(addr,bitnum))
#defineGPIOA_ODR_Addr(GPIOA_BASE+12)//0x4001080C
#defineGPIOB_ODR_Addr(GPIOB_BASE+12)//0x40010C0C
#defineGPIOC_ODR_Addr(GPIOC_BASE+12)//0x4001100C
#defineGPIOD_ODR_Addr(GPIOD_BASE+12)//0x4001140C
#defineGPIOE_ODR_Addr(GPIOE_BASE+12)//0x4001180C
#defineGPIOF_ODR_Addr(GPIOF_BASE+12)//0x40011A0C
#defineGPIOG_ODR_Addr(GPIOG_BASE+12)//0x40011E0C
//#definePAout(n)BIT_ADDR(GPIOA_ODR_Addr,n)//输出
//#definePAin(n)BIT_ADDR(GPIOA_IDR_Addr,n)//输入
#definePBout(n)BIT_ADDR(GPIOB_ODR_Addr,n)//输出
//#definePBin(n)BIT_ADDR(GPIOB_IDR_Addr,n)//输入
//#definePCout(n)BIT_ADDR(GPIOC_ODR_Addr,n)//输出
//#definePCin(n)BIT_ADDR(GPIOC_IDR_Addr,n)//输入
//#definePDout(n)BIT_ADDR(GPIOD_ODR_Addr,n)//输出
//#definePDin(n)BIT_ADDR(GPIOD_IDR_Addr,n)//输入
//#definePEout(n)BIT_ADDR(GPIOE_ODR_Addr,n)//输出
//#definePEin(n)BIT_ADDR(GPIOE_IDR_Addr,n)//输入
//#definePFout(n)BIT_ADDR(GPIOF_ODR_Addr,n)//输出
//#definePFin(n)BIT_ADDR(GPIOF_IDR_Addr,n)//输入
//#definePGout(n)BIT_ADDR(GPIOG_ODR_Addr,n)//输出
//#definePGin(n)BIT_ADDR(GPIOG_IDR_Addr,n)//输入
#definePI3.14159265
#defineZHONGZHI-6
#definePWMATIM1-》CCR1//PA8
#definePWMBTIM1-》CCR4//PA11
floatAngle_Balance,Gyro_Balance,Gyro_Turn;//平衡环控制相关变量
floatEncoder_left,Encoder_right;//速度环控制相关变量
intMoto1,Moto2;
/*
*函数名:main
*描述:主函数
*输入:无
*输出:无
*返回:无
*/
//中断分组处理函数
voidNVIC_Configuration(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置NVIC中断分组2:2位抢占优先级,2位响应优先级
}
intRead_Encoder(u8TIMX);
//位置平衡PID控制
intbalance(floatAngle,floatGyro)
{
floatBias,kp=500,kd=1;
intbalance;
Bias=Angle-ZHONGZHI;//===求出平衡的角度中值和机械相关
balance=kp*Bias+Gyro*kd;//===计算平衡控制的电机PWMPD控制kp是P系数kd是D系数
returnbalance;
}
//速度PI控制
intvelocity(intencoder_left,intencoder_right)
{
staticfloatVelocity,Encoder_Least,Encoder,Movement;
staticfloatEncoder_Integral,Target_Velocity;
floatkp=50,ki=kp/200;
Encoder_Least=(Encoder_left+Encoder_right)-0;
Encoder*=0.7;//一阶低通滤波,上次速度差占70,本次速度差占30,减缓速度差值,减少对直立系统的干扰
Encoder+=Encoder_Least*0.3;//一阶低通滤波
Encoder_Integral+=Encoder;//积分出位移,积分时间10ms
Encoder_Integral=Encoder_Integral-Movement;//接受遥控器的数据,控制正反转
if(Encoder_Integral》15000){
Encoder_Integral=15000;//积分限幅
}
if(Encoder_Integral《-15000)
{
Encoder_Integral=-15000;
}
Velocity=Encoder*kp+Encoder_Integral*ki;//PI控制器
returnVelocity;
}
//限幅函数
voidXianfu_Pwm(void)
{
intAmplitude=6900;//===PWM满幅是7200限制在6900
if(Moto1《-Amplitude)Moto1=-Amplitude;
if(Moto1》Amplitude)Moto1=Amplitude;
if(Moto2《-Amplitude)Moto2=-Amplitude;
if(Moto2》Amplitude)Moto2=Amplitude;
}
//绝对值函数
intmyabs(inta)
{
inttemp;
if(a《0)temp=-a;
elsetemp=a;
returntemp;
}
/*voidMiniBalance_Motor_Init(void)
{
RCC-》APB2ENR|=1《《3;//PORTB时钟使能
GPIOB-》CRH&=0X0000FFFF;//PORTB12131415推挽输出
GPIOB-》CRH|=0X22220000;//PORTB12131415推挽输出
}*/
voidMiniBalance_Motor_Init(void)
{
GPIO_InitTypeDefGPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//使能PB,PE端口时钟
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;//LED0--》PB.5端口配置
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;//推挽输出
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;//IO口速度为50MHz
GPIO_Init(GPIOB,&GPIO_InitStructure);//根据设定参数初始化GPIOB.5
//GPIO_SetBits(GPIOB,GPIO_Pin_5);
//GPIO_ResetBits(GPIOB,GPIO_Pin_6);//PB.5输出高
//GPIO_InitStructure.GPIO_Pin=GPIO_Pin_5;//LED1--》PE.5端口配置,推挽输出
//GPIO_Init(GPIOE,&GPIO_InitStructure);//推挽输出,IO口速度为50MHz
//GPIO_SetBits(GPIOE,GPIO_Pin_5);//PE.5输出高
}
voidMiniBalance_PWM_Init(u16arr,u16psc)
{
MiniBalance_Motor_Init();//初始化电机控制所需IO
RCC-》APB2ENR|=1《《11;//使能TIM1时钟
RCC-》APB2ENR|=1《《2;//PORTA时钟使能
GPIOA-》CRH&=0XFFFF0FF0;//PORTA811复用输出
GPIOA-》CRH|=0X0000B00B;//PORTA811复用输出
TIM1-》ARR=arr;//设定计数器自动重装值
TIM1-》PSC=psc;//预分频器不分频
TIM1-》CCMR2|=6《《12;//CH4PWM1模式
TIM1-》CCMR1|=6《《4;//CH1PWM1模式
TIM1-》CCMR2|=1《《11;//CH4预装载使能
TIM1-》CCMR1|=1《《3;//CH1预装载使能
TIM1-》CCER|=1《《12;//CH4输出使能
TIM1-》CCER|=1《《0;//CH1输出使能
TIM1-》BDTR|=1《《15;//TIM1必须要这句话才能输出PWM
TIM1-》CR1=0x8000;//ARPE使能
TIM1-》CR1|=0x01;//使能定时器1
}
//PWMshewzhi
voidSet_Pwm(intmoto1,intmoto2)
{
intsiqu=400;
if(moto1《0)
{
printf(“AIN反向”);
AIN1=0;
AIN2=1;
}
else
{
printf(“AINfanxaing”);
AIN2=0;
AIN1=1;
}
PWMA=myabs(moto1)+siqu;
if(moto2《0)
{
BIN1=0;
BIN2=1;
}
else
{
BIN1=1;
BIN2=0;
}
PWMB=myabs(moto2)+siqu;
}
intmain(void)
{
intAccel_Y,Accel_X,Accel_Z,Gyro_X,Gyro_Y,Gyro_Z;
floatAcceleration_Z;//Z轴加速度计
intBalance_Pwm,Velocity_Pwm;
NVIC_Configuration();//设置NVIC中断分组2:2位抢占优先级,2位响应优先级
/*串口1初始化*/
USART1_Config();
/*GPIO口初始化*/
MiniBalance_Motor_Init();
/*定时器1初始化*/
MiniBalance_PWM_Init(7199,0);
Encoder_Init_TIM2();//=====编码器接口
Encoder_Init_TIM4();//=====初始化编码器2
/*IIC接口初始化*/
I2C_MPU6050_Init();
/*陀螺仪传感器初始化*/
InitMPU6050();
/***********************************************************************/
while(1)
{
Accel_X=GetData(ACCEL_XOUT_H);
Accel_Y=GetData(ACCEL_YOUT_H);
Accel_Z=GetData(ACCEL_ZOUT_H);
Gyro_X=GetData(GYRO_XOUT_H);
Gyro_Y=GetData(GYRO_YOUT_H);
Gyro_Z=GetData(GYRO_ZOUT_H);
Encoder_left=-Read_Encoder(2);//===读取编码器的值,因为两个电机的旋转了180度的,所以对其中一个取反,保证输出极性一致
Encoder_right=Read_Encoder(4);
/*printf(“\r\n---------加速度X轴原始数据---------%d\r\n”,Accel_X);
printf(“\r\n---------加速度Y轴原始数据---------%d\r\n”,Accel_Y);
printf(“\r\n---------加速度Z轴原始数据---------%d\r\n”,Accel_Z);
printf(“\r\n---------陀螺仪X轴原始数据---------%d\r\n”,Gyro_X);
printf(“\r\n---------陀螺仪Y轴原始数据---------%d\r\n”,Gyro_Y);
printf(“\r\n---------陀螺仪Z轴原始数据---------%d\r\n”,Gyro_Z);*/
//delay_ms(500);
if(Gyro_Y》32768)Gyro_Y-=65536;//数据类型转换也可通过short强制类型转换
if(Gyro_Z》32768)Gyro_Z-=65536;//数据类型转换
if(Accel_X》32768)Accel_X-=65536;//数据类型转换
if(Accel_Z》32768)Accel_Z-=65536;//数据类型转换
Gyro_Balance=-Gyro_Y;//更新平衡角速度
Accel_Y=atan2(Accel_X,Accel_Z)*180/PI;//计算倾角
Gyro_Y=Gyro_Y/16.4;//陀螺仪量程转换
Kalman_Filter(Accel_Y,-Gyro_Y);//卡尔曼滤波
//elseif(Way_Angle==3)Yijielvbo(Accel_Y,-Gyro_Y);//互补滤波
Angle_Balance=angle;//更新平衡倾角
Gyro_Turn=Gyro_Z;//更新转向角速度
Acceleration_Z=Accel_Z;//===更新Z轴加速度计
Gyro_Balance=-Gyro_Y;
printf(“卡尔曼滤波值%f,%f\n”,Angle_Balance,Gyro_Turn);
//Balance_Pwm=balance(Angle_Balance,Gyro_Balance);
Velocity_Pwm=velocity(Encoder_left,Encoder_right);
Moto1=Velocity_Pwm;
Moto2=Velocity_Pwm;
printf(“%d,%d\n”,Moto1,Moto2);
Xianfu_Pwm();
Set_Pwm(Moto1,Moto2);
delay_ms(5);
}
}
/*******************(C)COPYRIGHT2012WildFireTeam*****ENDOFFILE************/
……………………
评论
查看更多