mpu6050卡爾曼濾波輸出姿態(tài)角程序

　　/********************（C）COPYRIGHT2012WildFireTeam**************************

　　*文件名：main.c

　　*描述：I2CEEPROM（AT24C02）測試，測試信息通過USART1打印在電腦的超級終端。

　　*實驗平臺：野火STM32開發(fā)板

　　*庫版本：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;//平衡環(huán)控制相關變量

　　floatEncoder_left，Encoder_right;//速度環(huán)控制相關變量

　　intMoto1，Moto2;

　　/*

　　*函數(shù)名：main

　　*描述：主函數(shù)

　　*輸入：無

　　*輸出：無

　　*返回：無

　　*/

　　//中斷分組處理函數(shù)

　　voidNVIC_Configuration（void）

　　{

　　NVIC_PriorityGroupConfig（NVIC_PriorityGroup_2）;//設置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應優(yōu)先級

　　}

　　intRead_Encoder（u8TIMX）;

　　//位置平衡PID控制

　　intbalance（floatAngle，floatGyro）

　　{

　　floatBias，kp=500，kd=1;

　　intbalance;

　　Bias=Angle-ZHONGZHI;//===求出平衡的角度中值和機械相關

　　balance=kp*Bias+Gyro*kd;//===計算平衡控制的電機PWMPD控制kp是P系數(shù)kd是D系數(shù)

　　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，減緩速度差值，減少對直立系統(tǒng)的干擾

　　Encoder+=Encoder_Least*0.3;//一階低通濾波

　　Encoder_Integral+=Encoder;//積分出位移，積分時間10ms

　　Encoder_Integral=Encoder_Integral-Movement;//接受遙控器的數(shù)據(jù)，控制正反轉

　　if（Encoder_Integral》15000）{

　　Encoder_Integral=15000;//積分限幅

　　}

　　if（Encoder_Integral《-15000）

　　{

　　Encoder_Integral=-15000;

　　}

　　Velocity=Encoder*kp+Encoder_Integral*ki;//PI控制器

　　returnVelocity;

　　}

　　//限幅函數(shù)

　　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;

　　}

　　//絕對值函數(shù)

　　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）;//根據(jù)設定參數(shù)初始化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;//設定計數(shù)器自動重裝值

　　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位搶占優(yōu)先級，2位響應優(yōu)先級

　　/*串口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軸原始數(shù)據(jù)---------%d\r\n”，Accel_X）;

　　printf（“\r\n---------加速度Y軸原始數(shù)據(jù)---------%d\r\n”，Accel_Y）;

　　printf（“\r\n---------加速度Z軸原始數(shù)據(jù)---------%d\r\n”，Accel_Z）;

　　printf（“\r\n---------陀螺儀X軸原始數(shù)據(jù)---------%d\r\n”，Gyro_X）;

　　printf（“\r\n---------陀螺儀Y軸原始數(shù)據(jù)---------%d\r\n”，Gyro_Y）;

　　printf（“\r\n---------陀螺儀Z軸原始數(shù)據(jù)---------%d\r\n”，Gyro_Z）;*/

　　//delay_ms（500）;

　　if（Gyro_Y》32768）Gyro_Y-=65536;//數(shù)據(jù)類型轉換也可通過short強制類型轉換

　　if（Gyro_Z》32768）Gyro_Z-=65536;//數(shù)據(jù)類型轉換

　　if（Accel_X》32768）Accel_X-=65536;//數(shù)據(jù)類型轉換

　　if（Accel_Z》32768）Accel_Z-=65536;//數(shù)據(jù)類型轉換

　　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************/

　　……………………