路

//****************************************
// Update to MPU6050 by shinetop
// MCU: STC89C52
// 2012.3.1
// 功能: 显示加速度计和陀螺仪的10位原始数据
//****************************************
// GY-52 MPU6050 IIC测试程序
// 使用单片机STC89C51
// 晶振：11.0592M
// 显示：LCD1602
// 编译环境 Keil uVision3
// 参考宏晶网站24c04通信程序
// 时间：2011年9月1日
// QQ：531389319
//****************************************
#include 
#include  //Keil library
#include  //Keil library
#include 
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//****************************************
// 定义51单片机端口
//****************************************
#define DataPort P0 //LCD1602数据端口
sbit SCL=P1^0; //IIC时钟引脚定义
sbit SDA=P1^1; //IIC数据引脚定义
sbit LCM_RS=P3^3; //LCD1602命令端口
sbit LCM_RW=P3^4; //LCD1602命令端口
sbit LCM_EN=P3^5; //LCD1602命令端口
//****************************************
// 定义MPU6050内部地址
//****************************************
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
//****************************************
//定义类型及变量
//****************************************
uchar dis[4]; //显示数字(-511至512)的字符数组
int dis_data; //变量
//int Temperature,Temp_h,Temp_l; //温度及高低位数据
//****************************************
//函数声明
//****************************************
void delay(unsigned int k); //延时
//LCD相关函数
void InitLcd(); //初始化lcd1602
void lcd_printf(uchar *s,int temp_data);
void WriteDataLCM(uchar dataW); //LCD数据
void WriteCommandLCM(uchar CMD,uchar Attribc); //LCD指令
void DisplayOneChar(uchar X,uchar Y,uchar DData); //显示一个字符
void DisplayListChar(uchar X,uchar Y,uchar *DData,L); //显示字符串
//MPU6050操作函数
void InitMPU6050(); //初始化MPU6050
void Delay5us();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
void display_ACCEL_x();
void display_ACCEL_y();
void display_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据
void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据
//****************************************
//整数转字符串
//****************************************
void lcd_printf(uchar *s,int temp_data)
{
if(temp_data<0)
{
temp_data=-temp_data;
*s='-';
}
else *s=' ';
*++s =temp_data/100+0x30;
temp_data=temp_data%100; //取余运算
*++s =temp_data/10+0x30;
temp_data=temp_data%10; //取余运算
*++s =temp_data+0x30;
}
//****************************************
//延时
//****************************************
void delay(unsigned int k)
{
unsigned int i,j;
for(i=0;i<k;i++)
{
for(j=0;j<121;j++);
}
}
//****************************************
//LCD1602初始化
//****************************************
void InitLcd()
{
WriteCommandLCM(0x38,1);
WriteCommandLCM(0x08,1);
WriteCommandLCM(0x01,1);
WriteCommandLCM(0x06,1);
WriteCommandLCM(0x0c,1);
DisplayOneChar(0,0,'A');
DisplayOneChar(0,1,'G');
}
//****************************************
//LCD1602写允许
//****************************************
void WaitForEnable(void)
{
DataPort=0xff;
LCM_RS=0;LCM_RW=1;_nop_();
LCM_EN=1;_nop_();_nop_();
while(DataPort&0x80);
LCM_EN=0;
}
//****************************************
//LCD1602写入命令
//****************************************
void WriteCommandLCM(uchar CMD,uchar Attribc)
{
if(Attribc)WaitForEnable();
LCM_RS=0;LCM_RW=0;_nop_();
DataPort=CMD;_nop_();
LCM_EN=1;_nop_();_nop_();LCM_EN=0;
}
//****************************************
//LCD1602写入数据
//****************************************
void WriteDataLCM(uchar dataW)
{
WaitForEnable();
LCM_RS=1;LCM_RW=0;_nop_();
DataPort=dataW;_nop_();
LCM_EN=1;_nop_();_nop_();LCM_EN=0;
}
//****************************************
//LCD1602写入一个字符
//****************************************
void DisplayOneChar(uchar X,uchar Y,uchar DData)
{
Y&=1;
X&=15;
if(Y)X|=0x40;
X|=0x80;
WriteCommandLCM(X,0);
WriteDataLCM(DData);
}
//****************************************
//LCD1602显示字符串
//****************************************
void DisplayListChar(uchar X,uchar Y,uchar *DData,L)
{
uchar ListLength=0;
Y&=0x1;
X&=0xF;
while(L--)
{
DisplayOneChar(X,Y,DData[ListLength]);
ListLength++;
X++;
}
}
//**************************************
//延时5微秒(STC90C52RC@12M)
//不同的工作环境,需要调整此函数
//当改用1T的MCU时,请调整此延时函数
//**************************************
void Delay5us()
{
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
}
//**************************************
//I2C起始信号
//**************************************
void I2C_Start()
{
SDA = 1; //拉高数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 0; //产生下降沿
Delay5us(); //延时
SCL = 0; //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop()
{
SDA = 0; //拉低数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 1; //产生上升沿
Delay5us(); //延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(bit ack)
{
SDA = ack; //写应答信号
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
//**************************************
//I2C接收应答信号
//**************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高时钟线
Delay5us(); //延时
CY = SDA; //读应答信号
SCL = 0; //拉低时钟线
Delay5us(); //延时
return CY;
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1; //移出数据的最高位
SDA = CY; //送数据口
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
I2C_RecvACK();
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1; //使能内部上拉,准备读取数据,
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1;
SCL = 1; //拉高时钟线
Delay5us(); //延时
dat |= SDA; //读数据
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
return dat;
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //内部寄存器地址，
I2C_SendByte(REG_data); //内部寄存器数据，
I2C_Stop(); //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //发送存储单元地址，从0开始
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号
REG_data=I2C_RecvByte(); //读出寄存器数据
I2C_SendACK(1); //接收应答信号
I2C_Stop(); //停止信号
return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//**************************************
//合成数据
//**************************************
int GetData(uchar REG_Address)
{
char H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H<<8)+L; //合成数据
}
//**************************************
//在1602上显示10位数据
//**************************************
void Display10BitData(int value,uchar x,uchar y)
{
value/=64; //转换为10位数据
lcd_printf(dis, value); //转换数据显示
DisplayListChar(x,y,dis,4); //启始列，行，显示数组，显示长度
}
//**************************************
//显示温度
//**************************************
//void display_temp()
//{
// Temp_h=Single_ReadI2C(TEMP_OUT_H); //读取温度
// Temp_l=Single_ReadI2C(TEMP_OUT_L); //读取温度
// Temperature=Temp_h<<8|Temp_l; //合成温度
// Temperature = 35+ ((double) (Temperature + 13200)) / 280; // 计算出温度
// lcd_printf(dis,Temperature); //转换数据显示
// DisplayListChar(11,1,dis,4); //启始列，行，显示数组，显示位数
//}
//*********************************************************
//主程序
//*********************************************************
void main()
{
delay(500); //上电延时
InitLcd(); //
InitMPU6050(); //初始化MPU6050
delay(150);
while(1)
{
Display10BitData(GetData(ACCEL_XOUT_H),2,0); //显示X轴加速度
Display10BitData(GetData(ACCEL_YOUT_H),7,0); //显示Y轴加速度
Display10BitData(GetData(ACCEL_ZOUT_H),12,0); //显示Z轴加速度
Display10BitData(GetData(GYRO_XOUT_H),2,1); //显示X轴角速度
Display10BitData(GetData(GYRO_YOUT_H),7,1); //显示Y轴角速度
Display10BitData(GetData(GYRO_ZOUT_H),12,1); //显示Z轴角速度
delay(500);
}
}

如果模块要求的电压与单片机电压不符合，那么就需要进行电平转换了，简单的可以串接一个1K的电阻，或者如图：

1N5819是一个正向压降为0.2V的肖特基二极管，可以保证低电平的电压够低。

#include <DS3231.h>
#include <Wire.h>

DS3231 Clock;
bool Century=false;
bool h12;
bool PM;
byte ADay, AHour, AMinute, ASecond, ABits;
bool ADy, A12h, Apm;
byte year, month, date, DoW, hour, minute, second;
int now;
//日出日落部分
float degreesToRadians = 3.1416 /180.0000;
float radiansToDegrees = 180.0000 /3.1416;
float degreeMinutesToDecimal = 1.0000 /60.0000;
float degreeSecondsToDecimal = 1.0000 /3600.0000;

bool input_East,input_South;
float input_Longitude,input_Latitude,input_Elevation,input_Month,input_Date,input_Year,input_TimeZone;//原始需要数据
int chu,luo,kai,guan;//结果
byte chu1,chu2,luo1,luo2;//结果转为小时、分钟
byte kai1,kai2,guan1,guan2;//结果转为小时、分钟
float NormalizeTo360 (float theThing) {
return (theThing - floor (theThing / 360.0) * 360);
}
void compute (void) {
float signedLongitude,signedLatitude,meridian,longitudeMeridianDifference,correctedYear,correctedMonth; //计算用转换数据
float t,G,C,L,alpha,obliquity,declination,eotAdjustment,clockTimeToLSOTAdjustment,sunRiseSetLSoTMinutes; //计算用中间数据
if (input_Latitude == 0) { input_Latitude = 0.000000001; }
if (input_Longitude == 0) { input_Longitude = 0.000000001; }
signedLongitude = input_Longitude;
if (input_East == 1) signedLongitude *= -1; // [1] = 东, [0] = 西
signedLatitude = input_Latitude;
if (input_South == 1) signedLatitude *= -1; // [0] = 北, [1] = 南
// 修复经度 > 180 deg
if (signedLongitude > 180) {
signedLongitude = signedLongitude - 360;
}
// 修复经度< -180 deg
if (signedLongitude < -180) {
signedLongitude = signedLongitude + 360;
}
// 当地标准时间子午线
meridian = input_TimeZone * -15;
// 如果太多与时区经度不同的警报
longitudeMeridianDifference = signedLongitude - meridian;
if ((longitudeMeridianDifference > 30) || (longitudeMeridianDifference < -30)) {
//alert ("所选择的时区与所在位置差距较大！");
}
// 计算通用时间
if (input_Month > 2) {
correctedYear = input_Year;
correctedMonth = input_Month - 3;
}
else {
correctedYear = input_Year - 1;
correctedMonth = input_Month + 9;
}
t = (input_Date + floor (30.6 * correctedMonth + 0.5) + floor (365.25 * (correctedYear - 1976)) - 8707.5) / 36525.0;
G = 357.528 + 35999.05 * t;
G = NormalizeTo360 (G);
C = (1.915 * sin (G * degreesToRadians)) + (0.020 * sin (2.0 * G * degreesToRadians));
L = 280.460 + (36000.770 * t) + C;
L = NormalizeTo360 (L);
alpha = L - 2.466 * sin (2.0 * L * degreesToRadians) + 0.053 * sin (4.0 * L * degreesToRadians);
obliquity = 23.4393 - 0.013 * t;
declination = atan (tan (obliquity * degreesToRadians) * sin (alpha * degreesToRadians)) * radiansToDegrees;
eotAdjustment = (L - C - alpha) / 15.0;
clockTimeToLSOTAdjustment = ((signedLongitude - meridian) / 15.0) - eotAdjustment; // 以小时为单位
sunRiseSetLSoTMinutes = radiansToDegrees * acos ( -1.0 * (sin (signedLatitude * degreesToRadians) * sin (declination * degreesToRadians) - sin ((-0.8333 - 0.0347 * sqrt (input_Elevation)) * degreesToRadians)) / cos (signedLatitude * degreesToRadians) / cos (declination * degreesToRadians)) * 4;
//日出时间
chu=12 * 60 - sunRiseSetLSoTMinutes + (clockTimeToLSOTAdjustment * 60);
//日落时间
luo=12 * 60 + sunRiseSetLSoTMinutes + (clockTimeToLSOTAdjustment * 60);
kai=luo+25;//日落后40分钟开灯
guan=chu-40;//日出前40分钟天亮，关灯
chu1=(int)chu/60;
chu2=(int)chu%60;
luo1=(int)luo / 60;
luo2=(int)luo % 60;
Serial.print("sunrise:");
Serial.print(chu1);
Serial.print(':');
Serial.print(chu2);
Serial.print(",sunset:");
Serial.print(luo1);
Serial.print(':');
Serial.print(luo2);
Serial.print('\n');
guan1=(int)(guan)/60;
guan2=(int)(guan)%60;
kai1=(int)(kai) / 60;
kai2=(int)(kai) % 60;
Serial.print("break:");
Serial.print(guan1);
Serial.print(':');
Serial.print(guan2);
Serial.print(",dark:");
Serial.print(kai1);
Serial.print(':');
Serial.print(kai2);
Serial.print('\n');
}

void setup() {
pinMode( 2 , OUTPUT);
digitalWrite( 2 , LOW );
// Start the I2C interface
Wire.begin();
//Clock.setSecond(50);//Set the second
//Clock.setMinute(36);//Set the minute
//Clock.setHour(8); //Set the hour
//Clock.setDoW(6); //Set the day of the week
//Clock.setDate(10); //Set the date of the month
//Clock.setMonth(3); //Set the month of the year
//Clock.setYear(17); //Set the year (Last two digits of the year)
// Start the serial interface
Serial.begin(115200);
//提供的参数
input_Longitude=114.41; //经度
input_East=1;//东经1，西经0
input_Latitude=36.93;//维度
input_South=0;//北纬0，南纬1
//input_Year=2017;
//input_Month=2;
//input_Date=2;
input_TimeZone=8;
}
byte decToBcd(byte val) {
// Convert normal decimal numbers to binary coded decimal
return ( (val/10*16) + (val%10) );
}

byte bcdToDec(byte val) {
// Convert binary coded decimal to normal decimal numbers
return ( (val/16*10) + (val%16) );
}
void ReadDS3231()
{
int second,minute,hour,date,month,year,temperature;
second=Clock.getSecond();
minute=Clock.getMinute();
hour=Clock.getHour(h12, PM);
date=Clock.getDate();
month=Clock.getMonth(Century);
year=Clock.getYear();

temperature=Clock.getTemperature();
// input_Year=2017;
//input_Month=3;
//input_Date=10;
input_Year=2000+year;
input_Month=month;
input_Date=date;
compute();//计算
now=hour*60+minute;
//Serial.print(input_Year);
//Serial.print('\n');
//Serial.print(now);
//Serial.print('-');
//Serial.print(kai);
//Serial.print('-');
//Serial.print(guan);
//Serial.print('\n');
if (now>guan&&now<kai){
digitalWrite( 2 , LOW );
}else{
digitalWrite( 2 , HIGH );
}
Serial.print("now:20");
Serial.print(year,DEC);
Serial.print('-');
Serial.print(month,DEC);
Serial.print('-');
Serial.print(date,DEC);
Serial.print(' ');
Serial.print(hour,DEC);
Serial.print(':');
Serial.print(minute,DEC);
Serial.print(':');
Serial.print(second,DEC);
Serial.print('\n');
Serial.print("Temperature=");
Serial.print(temperature);
Serial.print('\n');
Serial.print('\n');
Serial.print('\n');
}
void loop() {ReadDS3231();delay(10000);

}

http://www.pengzhihui.xyz/

STC15的PWM输出频率计算：

PCA初始化时，PCA_Clock_1T时，频率为系统频率/256。

22.1184M时，PWM为86.4K Hz

我要一个更低的频率怎么做呢？

PCA_Clock_12T 频率也为7.2K，只有降低时钟频率了，最低降为5.5296M的时候，5.5296M/256/12为1.8K。

现在，只能再进行分频了，SYSTEM_CLK_4T()，使系统时钟变为1.3824M，这样，PWM频率为540Hz

程序：

#include "config.h"
#include "delay.h"
#include "PCA.h"
#include "ADC.h"
/************* 本地函数声明 **************/
void timer0_int (void) interrupt TIMER0_VECTOR //timer0用来调度定时任务。
{
UpdatePwm(PCA0,255-GetADCResult(0));
UpdatePwm(PCA1,255-GetADCResult(1));
UpdatePwm(PCA2,255-GetADCResult(2));
WDT_reset(4);//喂狗
}
/**********************************************/
void main(void)
{
SYSTEM_CLK_4T();
Timer0_16bitAutoReload();//16位模式
Timer0_AsTimer();//定时器模式
Timer0_1T();
TL0 = 0x9A; //设置定时初值1ms
TH0 = 0xA9; //设置定时初值
Timer0_InterruptEnable();//使能定时器0
PCA_config(); //PWM配置
InitADC(); //初始化ADC
EA = 1;//使能总中断
delay_ms(1);
delay_3us(1);
delay_us(1);
Timer0_Run(); //完成初始化定时器0开始计时
while (1)
{
}

}

http://www.51hei.com/bbs/dpj-41904-1.html

Ardublock是一个图形化的arduino编程软件，非常好用。

Ardublock教育版

教育版相比功能更多一些。支持子程序等。还是很不错的。但是据说不支持1.6.5以上版本的IDE。

标准版的ardublock?tools?这是网上通常用的版本

安装方法：

windows下：放到IDE的目录下路径“tools\ArduBlockTool\tool”区分大小写。

记得没多久前还用一款串口调试助手来画曲线，现在却怎么也不知道软件的名字了。无奈！

想看原始数据的变化，SerialChart_v034这款软件试了试，的确还不错。

打开后需要自己写下配置文件

[_setup_]

port=COM3

baudrate=115200

width=1000

height=400

background_color = white

grid_h_origin =100

grid_h_step = 10

grid_h_color = #EEE

grid_h_origin_color = #CCC

grid_v_origin = 100

grid_v_step = 10

grid_v_color = #EEE

grid_v_origin_color=transparent

[_default_]

min=-0.3

max=1.5

[1]

color = green

[2]

color = blue

[3]

color = red

[4]

color = green

[5]

color = blue

[6]

color = red

上边这段代码就是6个数据的曲线，发过来一条逗号分割的6个数据就可以自动画出曲线了。性能也不错。

很多好的网址时间长了都忘记了。这个不错的网址以后都要记录下来：

http://blog.csdn.net/ling3ye/article/details/51360568 ? ?Arduino uno + mpu6050 陀螺仪 运用卡尔曼滤波姿态解算实验

MPU6050六轴陀螺仪

作用于四轴无人机，平衡车，机器人等等的电子实作当中，用于姿态判断，掌握了可以发挥自己的想象完成更多更有趣的作品。

本例程输出XYZ的角度，正负90度。

运用卡尔曼滤波算法解算姿态，感觉算是比较稳定，但好像有点偏移。大家好好学习参考，再改进吧。

输出效果

首先看看本例程XYZ轴的输出效果图:

（时间曲线的体现是：静止姿态→摆动→恢复原静止姿态→拍动桌子→静止姿态）

Bom表

Arduino Uno ? ? ? ? ? ? ? *1

mpu6050 陀螺仪模块 *1

跳线 ? ? ? ? ? ? ? ? ? ? ? ? ? ?若干

MPU6050 引脚说明

VCC ? ? ? ? ? ? 3.3-5V（内部有稳压芯片）

GND ? ? ? ? ? ? 地线
SCL ? ? ? ? ? ? ?MPU6050作为从机时IIC时钟线

SDA ? ? ? ? ? ? ?MPU6050作为从机时IIC数据线

XCL ? ? ? ? ? ? ? MPU6050作为主机时IIC时钟线

XDA ? ? ? ? ? ? ?MPU6050作为主机时IIC数据线
AD0 ? ? ? ? ? ? ?地址管脚，该管脚决定了IIC地址的最低一位

INT ? ? ? ? ? ? ? ?中断引脚

接线

Arduino uno+MPU6050接线方式如下

程序实现

首先要更新I2C库

在GITHUB找到的I2C库

（程序来源:?https://github.com/jrowberg/i2cdevlib）

打开,把Arduino文件夹里的I2Cdev，MPU6050文件夹复制到Arduino IDE的库文件夹里

(默认的路径是这个 C:\Program Files (x86)\Arduino\libraries)

在GITHUB找到的卡尔曼滤波程序（程序来源: https://github.com/wjjun/MPU6050_Kalman）

把程序上传到板子上，打开串口监视器，就可以看到一堆堆的数据了

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;

unsigned long now, lastTime = 0;
float dt; //微分时间

int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0; //角度变量
long axo = 0, ayo = 0, azo = 0; //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0; //陀螺仪偏移量

float pi = 3.1415926;
float AcceRatio = 16384.0; //加速度计比例系数
float GyroRatio = 131.0; //陀螺仪比例系数

uint8_t n_sample = 8; //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0}; //x,y轴采样队列
long aax_sum, aay_sum,aaz_sum; //x,y轴采样和

float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0}; //加速度计协方差计算队列
float Px=1, Rx, Kx, Sx, Vx, Qx; //x轴卡尔曼变量
float Py=1, Ry, Ky, Sy, Vy, Qy; //y轴卡尔曼变量
float Pz=1, Rz, Kz, Sz, Vz, Qz; //z轴卡尔曼变量

void setup()
{
Wire.begin();
Serial.begin(115200);

accelgyro.initialize(); //初始化

unsigned short times = 200; //采样次数
for(int i=0;i<times;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
axo += ax; ayo += ay; azo += az; //采样和
gxo += gx; gyo += gy; gzo += gz;

}

axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
}

void loop()
{
unsigned long now = millis(); //当前时间(ms)
dt = (now - lastTime) / 1000.0; //微分时间(s)
lastTime = now; //上一次采样时间(ms)

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值

float accx = ax / AcceRatio; //x轴加速度
float accy = ay / AcceRatio; //y轴加速度
float accz = az / AcceRatio; //z轴加速度

aax = atan(accy / accz) * (-180) / pi; //y轴对于z轴的夹角
aay = atan(accx / accz) * 180 / pi; //x轴对于z轴的夹角
aaz = atan(accz / accy) * 180 / pi; //z轴对于y轴的夹角

aax_sum = 0; // 对于加速度计原始数据的滑动加权滤波算法
aay_sum = 0;
aaz_sum = 0;

for(int i=1;i<n_sample;i++)
{
aaxs[i-1] = aaxs[i];
aax_sum += aaxs[i] * i;
aays[i-1] = aays[i];
aay_sum += aays[i] * i;
aazs[i-1] = aazs[i];
aaz_sum += aazs[i] * i;

}

aaxs[n_sample-1] = aax;
aax_sum += aax * n_sample;
aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度调幅至0-90°
aays[n_sample-1] = aay; //此处应用实验法取得合适的系数
aay_sum += aay * n_sample; //本例系数为9/7
aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
aazs[n_sample-1] = aaz;
aaz_sum += aaz * n_sample;
aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;

float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
float gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度
agx += gyrox; //x轴角速度积分
agy += gyroy; //x轴角速度积分
agz += gyroz;

/* kalman start */
Sx = 0; Rx = 0;
Sy = 0; Ry = 0;
Sz = 0; Rz = 0;

for(int i=1;i<10;i++)
{ //测量值平均值运算
a_x[i-1] = a_x[i]; //即加速度平均值
Sx += a_x[i];
a_y[i-1] = a_y[i];
Sy += a_y[i];
a_z[i-1] = a_z[i];
Sz += a_z[i];

}

a_x[9] = aax;
Sx += aax;
Sx /= 10; //x轴加速度平均值
a_y[9] = aay;
Sy += aay;
Sy /= 10; //y轴加速度平均值
a_z[9] = aaz;
Sz += aaz;
Sz /= 10;

for(int i=0;i<10;i++)
{
Rx += sq(a_x[i] - Sx);
Ry += sq(a_y[i] - Sy);
Rz += sq(a_z[i] - Sz);

}

Rx = Rx / 9; //得到方差
Ry = Ry / 9;
Rz = Rz / 9;

Px = Px + 0.0025; // 0.0025在下面有说明...
Kx = Px / (Px + Rx); //计算卡尔曼增益
agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加
Px = (1 - Kx) * Px; //更新p值

Py = Py + 0.0025;
Ky = Py / (Py + Ry);
agy = agy + Ky * (aay - agy);
Py = (1 - Ky) * Py;

Pz = Pz + 0.0025;
Kz = Pz / (Pz + Rz);
agz = agz + Kz * (aaz - agz);
Pz = (1 - Kz) * Pz;

/* kalman end */
Serial.print(accx);Serial.print(",");
Serial.print(accy);Serial.print(",");
Serial.print(accz);Serial.print(",");
Serial.print(gyrox);Serial.print(",");
Serial.print(gyroy);Serial.print(",");
Serial.print(gyroz);Serial.print(",");
Serial.print(agx);Serial.print(",");
Serial.print(agy);Serial.print(",");
Serial.print(agz);Serial.println();

}

想用MPU6050来测试振动值，振动值应该从MPU6050的加速度值可以获取到。mpu6050原始输出的6个信息为xyz三轴的加速度和角速度。并不能输出现在的姿态。用卡尔曼滤波得到的数据是姿态数据。

器件中文资料

MPU-6000.6050中文资料

MPU6050中文寄存器手册

MPU6050教程