mpu6050卡尔曼滤波输出姿态角程序 -

/******************** (C) COPYRIGHT 2012 WildFire Team **************************
* 文件名：main.c
* 描述：I2C EEPROM(AT24C02)测试，测试信息通过USART1打印在电脑的超级终端。
* 实验平台：野火STM32开发板
* 库版本：ST3.0.0
* 作者：wildfire team
**********************************************************************************/
#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"
#define AIN2 PBout(15)
#define AIN1 PBout(14)
#define BIN1 PBout(13)
#define BIN2 PBout(12)
#define BITBAND(addr, bitnum) ((addr & 0xF0000000)+0x2000000+((addr &0xFFFFF)<<5)+(bitnum<<2))
#define MEM_ADDR(addr) *((volatile unsigned long *)(addr))
#define BIT_ADDR(addr, bitnum) MEM_ADDR(BITBAND(addr, bitnum))
#define GPIOA_ODR_Addr (GPIOA_BASE+12) //0x4001080C
#define GPIOB_ODR_Addr (GPIOB_BASE+12) //0x40010C0C
#define GPIOC_ODR_Addr (GPIOC_BASE+12) //0x4001100C
#define GPIOD_ODR_Addr (GPIOD_BASE+12) //0x4001140C
#define GPIOE_ODR_Addr (GPIOE_BASE+12) //0x4001180C
#define GPIOF_ODR_Addr (GPIOF_BASE+12) //0x40011A0C
#define GPIOG_ODR_Addr (GPIOG_BASE+12) //0x40011E0C
//#define PAout(n) BIT_ADDR(GPIOA_ODR_Addr,n) //输出
//#define PAin(n) BIT_ADDR(GPIOA_IDR_Addr,n) //输入
#define PBout(n) BIT_ADDR(GPIOB_ODR_Addr,n) //输出
//#define PBin(n) BIT_ADDR(GPIOB_IDR_Addr,n) //输入
//#define PCout(n) BIT_ADDR(GPIOC_ODR_Addr,n) //输出
//#define PCin(n) BIT_ADDR(GPIOC_IDR_Addr,n) //输入
//#define PDout(n) BIT_ADDR(GPIOD_ODR_Addr,n) //输出
//#define PDin(n) BIT_ADDR(GPIOD_IDR_Addr,n) //输入
//#define PEout(n) BIT_ADDR(GPIOE_ODR_Addr,n) //输出
//#define PEin(n) BIT_ADDR(GPIOE_IDR_Addr,n) //输入
//#define PFout(n) BIT_ADDR(GPIOF_ODR_Addr,n) //输出
//#define PFin(n) BIT_ADDR(GPIOF_IDR_Addr,n) //输入
//#define PGout(n) BIT_ADDR(GPIOG_ODR_Addr,n) //输出
//#define PGin(n) BIT_ADDR(GPIOG_IDR_Addr,n) //输入
#define PI 3.14159265
#define ZHONGZHI -6
#define PWMA TIM1->CCR1 //PA8
#define PWMB TIM1->CCR4 //PA11
float Angle_Balance,Gyro_Balance,Gyro_Turn; //平衡环控制相关变量
float Encoder_left,Encoder_right; //速度环控制相关变量
int Moto1,Moto2;
/*
* 函数名：main
* 描述：主函数
* 输入：无
* 输出：无
* 返回：无
*/
//中断分组处理函数
void NVIC_Configuration(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
}
int Read_Encoder(u8 TIMX);
//位置平衡PID控制
int balance(float Angle,float Gyro)
{
float Bias,kp=500,kd=1;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值和机械相关
balance=kp*Bias+Gyro*kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return balance;
}
//速度PI控制
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral,Target_Velocity;
float kp=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 控制器
return Velocity;
}
//限幅函数
void Xianfu_Pwm(void)
{
int Amplitude=6900; //===PWM满幅是7200 限制在6900
if(Moto1<-Amplitude) Moto1=-Amplitude;
if(Moto1>Amplitude) Moto1=Amplitude;
if(Moto2<-Amplitude) Moto2=-Amplitude;
if(Moto2>Amplitude) Moto2=Amplitude;
}
//绝对值函数
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/*void MiniBalance_Motor_Init(void)
{
RCC->APB2ENR|=1<<3; //PORTB时钟使能
GPIOB->CRH&=0X0000FFFF; //PORTB12 13 14 15推挽输出
GPIOB->CRH|=0X22220000; //PORTB12 13 14 15推挽输出
}*/
void MiniBalance_Motor_Init(void)
{
GPIO_InitTypeDef GPIO_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 输出高
}
void MiniBalance_PWM_Init(u16 arr,u16 psc)
{
MiniBalance_Motor_Init(); //初始化电机控制所需IO
RCC->APB2ENR|=1<<11; //使能TIM1时钟
RCC->APB2ENR|=1<<2; //PORTA时钟使能
GPIOA->CRH&=0XFFFF0FF0; //PORTA8 11复用输出
GPIOA->CRH|=0X0000B00B; //PORTA8 11复用输出
TIM1->ARR=arr; //设定计数器自动重装值
TIM1->PSC=psc; //预分频器不分频
TIM1->CCMR2|=6<<12; //CH4 PWM1模式
TIM1->CCMR1|=6<<4; //CH1 PWM1模式
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
}
//PWM shewzhi
void Set_Pwm(int moto1,int moto2)
{
int siqu=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;
}
int main(void)
{
int Accel_Y,Accel_X,Accel_Z,Gyro_X,Gyro_Y,Gyro_Z;
float Acceleration_Z; //Z轴加速度计
int Balance_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);//卡尔曼滤波
//else if(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) COPYRIGHT 2012 WildFire Team *****END OF FILE************/
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码