|
|
#include <REG52.H>
#include <math.h> //Keil library
#include <stdio.h> //Keil library
#include <INTRINS.H>
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//******功能模塊頭文件*******
//#include "DELAY.H" //延時(shí)頭文件
#include "lcd1602.H" //LCD1602文件
#include "MPU6050.H" //MPU6050頭文件
//******角度參數(shù)************
float Accel_ax;
float Accel_az; //X軸加速度值暫存
float Angle; //小車最終傾斜角度
uchar value; //角度正負(fù)極性標(biāo)記
//float Accel_x;
//float Angle_ax;
//float Gyro_y; //Y軸陀螺儀數(shù)據(jù)暫存
//*********************************************************
// 傾角計(jì)算（卡爾曼融合）
//*********************************************************
float Angle_Calcu(void)
{
//------加速度--------------------------
//范圍為2g時(shí)，換算關(guān)系：16384 LSB/g
//角度較小時(shí)，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因?yàn)閤>=sinx,故乘以1.3適當(dāng)放大
Accel_ax = GetData(ACCEL_XOUT_H); //讀取X軸加速度
Accel_az = GetData(ACCEL_ZOUT_H); //讀取X軸加速度
Angle = (int)(atan(Accel_ax/Accel_az)*180/3.14);
/* Accel_x = GetData(ACCEL_XOUT_H); //讀取X軸加速度
Angle_ax = (Accel_x - 1100) /16384; //去除零點(diǎn)偏移,計(jì)算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度轉(zhuǎn)換為度,
return Angle_ax;
Gyro_y = GetData(GYRO_YOUT_H); //靜止時(shí)角速度Y軸輸出為-30左右
Gyro_y = -(Gyro_y + 30)/16.4; //去除零點(diǎn)偏移，計(jì)算角速度值,負(fù)號為方向處理
Angle = Angle + Gyro_y*0.01; //角速度積分得到傾斜角度.*/
return Angle;
}
void main()
{ //float ax,ay,az,xx,yy,zz;
float xx;
delay(500); //上電延時(shí)
InitLcd(); //液晶初始化
InitMPU6050(); //初始化MPU6050
delay(150);
while(1)
{
xx=Angle_Calcu();
lcd_printf(dis,xx); //轉(zhuǎn)換數(shù)據(jù)顯示
DisplayListChar(2,0,dis,4);
}
}
這是我寫的程序希望能解決 |
|
[復(fù)制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
