#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" //延时头文件 #include "lcd1602.H" //LCD1602文件 #include "MPU6050.H" //MPU6050头文件 //******角度参数************ float Accel_ax; float Accel_az; //X轴加速度值暂存 float Angle; //小车最终倾斜角度 uchar value; //角度正负极性标记 //float Accel_x; //float Angle_ax; //float Gyro_y; //Y轴陀螺仪数据暂存 //********************************************************* // 倾角计算（卡尔曼融合） //********************************************************* float Angle_Calcu(void) { //------加速度-------------------------- //范围为2g时，换算关系：16384 LSB/g //角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14 //因为x>=sinx,故乘以1.3适当放大 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; //去除零点偏移,计算得到角度（弧度） Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度, return Angle_ax; Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y轴输出为-30左右 Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理 Angle = Angle + Gyro_y*0.01; //角速度积分得到倾斜角度.*/ return Angle; } void main() { //float ax,ay,az,xx,yy,zz; float xx; delay(500); //上电延时 InitLcd(); //液晶初始化 InitMPU6050(); //初始化MPU6050 delay(150); while(1) { xx=Angle_Calcu(); lcd_printf(dis,xx); //转换数据显示 DisplayListChar(2,0,dis,4); } } |