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

ID:159760 · 發(fā)表于 2017-7-30 01:31

經(jīng)過調(diào)試可以正確讀取數(shù)據(jù)并輸出姿態(tài)角

單片機源程序如下:

/******************** (C) COPYRIGHT 2012 WildFire Team **************************
* 文件名：main.c
* 描述：I2C EEPROM(AT24C02)測試，測試信息通過USART1打印在電腦的超級終端。
* 實驗平臺：野火STM32開發(fā)板
* 庫版本：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; //平衡環(huán)控制相關(guān)變量
float Encoder_left,Encoder_right; //速度環(huán)控制相關(guān)變量
int Moto1,Moto2;
/*
* 函數(shù)名：main
* 描述：主函數(shù)
* 輸入：無
* 輸出：無
* 返回：無
*/
//中斷分組處理函數(shù)
void NVIC_Configuration(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應(yīng)優(yōu)先級
}
int Read_Encoder(u8 TIMX);
//位置平衡PID控制
int balance(float Angle,float Gyro)
{
float Bias,kp=500,kd=1;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值和機械相關(guān)
balance=kp*Bias+Gyro*kd; //===計算平衡控制的電機PWM PD控制 kp是P系數(shù) kd是D系數(shù)
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，減緩速度差值，減少對直立系統(tǒng)的干擾
Encoder+=Encoder_Least*0.3; //一階低通濾波
Encoder_Integral+=Encoder; //積分出位移，積分時間10ms
Encoder_Integral=Encoder_Integral-Movement; //接受遙控器的數(shù)據(jù)，控制正反轉(zhuǎn)
if(Encoder_Integral>15000){
Encoder_Integral=15000; //積分限幅
}
if(Encoder_Integral<-15000)
{
Encoder_Integral=-15000;
}
Velocity=Encoder*kp+Encoder_Integral*ki; //PI 控制器
return Velocity;
}
//限幅函數(shù)
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;
}
//絕對值函數(shù)
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); //根據(jù)設(shè)定參數(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 輸出高
}
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復(fù)用輸出
GPIOA->CRH|=0X0000B00B; //PORTA8 11復(fù)用輸出
TIM1->ARR=arr; //設(shè)定計數(shù)器自動重裝值
TIM1->PSC=psc; //預(yù)分頻器不分頻
TIM1->CCMR2|=6<<12; //CH4 PWM1模式
TIM1->CCMR1|=6<<4; //CH1 PWM1模式
TIM1->CCMR2|=1<<11; //CH4預(yù)裝載使能
TIM1->CCMR1|=1<<3; //CH1預(yù)裝載使能
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(); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應(yīng)優(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); //===讀取編碼器的值，因為兩個電機的旋轉(zhuǎn)了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ù)類型轉(zhuǎn)換也可通過short強制類型轉(zhuǎn)換
if(Gyro_Z>32768) Gyro_Z-=65536; //數(shù)據(jù)類型轉(zhuǎn)換
if(Accel_X>32768) Accel_X-=65536; //數(shù)據(jù)類型轉(zhuǎn)換
if(Accel_Z>32768) Accel_Z-=65536; //數(shù)據(jù)類型轉(zhuǎn)換
Gyro_Balance=-Gyro_Y; //更新平衡角速度
Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //計算傾角
Gyro_Y =Gyro_Y/16.4; //陀螺儀量程轉(zhuǎn)換
Kalman_Filter(Accel_Y,-Gyro_Y);//卡爾曼濾波
//else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //互補濾波
Angle_Balance=angle; //更新平衡傾角
Gyro_Turn=Gyro_Z; //更新轉(zhuǎn)向角速度
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黑下載附件…………

復(fù)制代碼

所有資料51hei提供下載:

卡爾曼濾波輸出姿態(tài)角.zip (2.14 MB, 下載次數(shù): 431)

ID:217957 · 發(fā)表于 2018-1-6 14:54

資料不錯，參考一下

ID:128321 · 發(fā)表于 2018-3-21 21:37

mfc20010 發(fā)表于 2018-1-6 14:54
資料不錯，參考一下

資料不錯，參考一下

ID:237206 · 發(fā)表于 2018-3-25 20:13

支持一下

ID:290380 · 發(fā)表于 2018-5-3 21:18

資料不錯，參考一下，支持一下

ID:323802 · 發(fā)表于 2018-5-6 21:52

資料不錯哦

ID:329263 · 發(fā)表于 2018-5-14 01:48

可以把附件發(fā)給我嘛，676913753@qq.com沒有幣很難受

ID:320612 · 發(fā)表于 2018-5-22 15:55

資料不錯，參考一下，支持一下

ID:337376 · 發(fā)表于 2018-6-5 17:21

很好的算法，謝謝分享

ID:320161 · 發(fā)表于 2018-6-9 23:44

不能用騙人的!

ID:368329 · 發(fā)表于 2018-7-9 23:43

資料不錯，參考一下，支持一下

ID:158903 · 發(fā)表于 2018-7-10 16:10

資料不錯，參考一下，支持一下
馬克

ID:370943 · 發(fā)表于 2018-9-26 15:33

學習大神的作品

ID:248291 · 發(fā)表于 2018-9-29 22:46

學習一下

ID:387286 · 發(fā)表于 2018-11-5 19:52

不能用，代碼不全

ID:502704 · 發(fā)表于 2019-4-2 18:47

資料不錯，參考一下

ID:442220 · 發(fā)表于 2019-7-29 19:42

學習了，最近急需這些

ID:267330 · 發(fā)表于 2019-7-30 08:58

謝謝正是我需要的

ID:421308 · 發(fā)表于 2019-8-2 08:54

資料不錯，學習下

ID:588621 · 發(fā)表于 2019-8-2 15:29

厲害哦咯可以用下

ID:443082 · 發(fā)表于 2020-6-4 23:54

資料不錯，可以放到stm32f103c8t6上使用并且成功輸出。感謝分享。

ID:369024 · 發(fā)表于 2020-6-7 12:27

沒有姿態(tài)結(jié)算

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

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