#include?<Adafruit_SSD1306.h>
#include?"MPU6050.h"??//頭文件
#include?"I2Cdev.h"
//OLED相關(guān)
#define?OLED_RESET?4
Adafruit_SSD1306?display(OLED_RESET);??//OLED引腳設(shè)置



#define?N_??5
unsigned?char?i=0;
int16_t?ax,?ay,?az;??//初始化變量
int16_t?gx,?gy,?gz;
int32_t?iax=0,iay=0,iaz=0;
int32_t?igx=0,igy=0,igz=0;
int16_t?cdata=0;
MPU6050?mpu;??//創(chuàng)建結(jié)構(gòu)體名mpu

void?setup()?{
??Wire.begin();??//加入?I2C?總線序列
??Serial.begin(9600);??//開啟串口，設(shè)置波特率
??delay(1000);
??mpu.initialize();??//庫(kù)初始化
??mpu_init2();??//獲取偏移量修正
??display.begin(SSD1306_SWITCHCAPVCC,?0x3C);??//初始化OLED?IIC總線
??display.clearDisplay();???//?清屏
??display.setTextSize(1);
??display.setTextColor(WHITE);
??display.setCursor(0,?0);
}
void?loop()?{

??mpu.getMotion6(&ax,?&ay,?&az,?&gx,?&gy,?&gz);?
??counter_data();??//數(shù)據(jù)計(jì)算
??test();
??delay(300);
}
void?test(void)
{
???display.clearDisplay();??//?清屏
??display.setCursor(30,?0);
??display.print("MPU6050?Test");

??display.setCursor(30,?20);
??display.print("X:");
??display.println(ax);

??display.setCursor(30,?30);
??display.print("Y:");
??display.println(ay);

??display.setCursor(30,?40);
??display.print("Z:");
??display.println(az);

??display.display();
}
void?mpu_init2(){
??unsigned?char?i=0,j=0;
??for(i=0;i<N_;i++){
????mpu.getMotion6(&ax,?&ay,?&az,?&gx,?&gy,?&gz);????
????iax=iax+ax;iay=iay+ay;iaz=iaz+az;
????igx=igx+gx;igy=igy+gy;igz=igz+gz;
??}
??iax=iax/N_;iay=iay/N_;iaz=iaz/N_;
??igx=igx/N_;igy=igy/N_;igz=igz/N_;
}
void?counter_data(){????????????????????//將獲取到的數(shù)據(jù)進(jìn)行處理和計(jì)算
??
??ax=ax-iax;ay=ay-iay;az=az-iaz;????????//計(jì)算偏移值
??gx=gx-igx;gy=gy-igy;gz=gz-igz;
??ax=ax/180;ay=ay/180;az=az/180;????????
??Serial.print("mov_X=");Serial.print(ax);Serial.print("°");Serial.print("\t");
??Serial.print("mov_Y=");Serial.print(ay);Serial.print("°");Serial.print("\t");
??Serial.print("mov_Z=");Serial.print(az);Serial.print("°");Serial.print("\n");

}