新 Nano（四）MPU6050

程序上传后，就可以看到一个小方框再屏幕中乱跑。嗯。。。挺减压的。

 #include 
 #include 
 #include 
 #include 

 U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);        
 //U8GLIB_SH1106_128X64 u8g(13, 11, 10, 9);
 //U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0|U8G_I2C_OPT_NO_ACK|U8G_I2C_OPT_FAST);        // Fast I2C / TWI 
 // HW SPI Com: CS = 10, A0 = 9 (Hardware Pins are  SCK = 13 and MOSI = 11)

 //====set offset of x,y,z===========
 #define Gx_offset -3.06 
 #define Gy_offset 1.01
 #define Gz_offset -0.88

 #define LED0 8
 #define LED1 9
 #define LED2 10
 #define LED3 11

 //====================
 MPU6050 accelgyro;

 int16_t ax,ay,az;
 int16_t gx,gy,gz;//Storing original data
 float aax,aay,aaz,ggx,ggy,ggz;//The quantized data is stored
 float Ax,Ay,Az;// g(9.8m/s^2)
 float Gx,Gy,Gz;// ��/s
 int left,right,head,back;
 int Mx,My,Mz,Nx,Ny,Nz;

 float Angel_accX,Angel_accY,Angel_accZ,A,B,C;//Storing the calculated angle by acceleration

 long LastTime,NowTime,TimeSpan;//Integral to the angular velocity


 void setup()
  {
   Wire.begin();

   Serial.begin(115200);

   Serial.println("Initializing I2C device.....");
   accelgyro.initialize();

   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful":"MPU6050 connection failure");

   pinMode(LED0,OUTPUT);
   pinMode(LED1,OUTPUT);
   pinMode(LED2,OUTPUT);
   pinMode(LED3,OUTPUT);
   }

 void u8g_box_frame() 
  {
   //u8g.drawLine(90,35,Mz,Nz);//z
   //u8g.drawLine(90,35,My,Ny);//y
   //u8g.drawLine(90,35,80,50);//x
   B = -Angel_accX+32;
   A = -Angel_accY+64;
   u8g.drawFrame(A, B, 20, 20);
   }


 void Rotate() {
 if((-90<=Angel_accY)&&(Angel_accY<=0)&&(0<=Angel_accZ)&&(Angel_accZ<=90))
 {
 My=90+30*cos(Angel_accY);
 Ny=35-30*sin(Angel_accY);
 Mz=90+30*cos(Angel_accZ);
 Nz=35-30*sin(Angel_accZ);
 }
 else if((-90<=Angel_accY)&&(Angel_accY<=0)&&(-90<=Angel_accZ)&&(Angel_accZ<=0))
 {
 My=90+30*cos(Angel_accY);
 Ny=35-30*sin(Angel_accY);
 Mz=90+30*cos(Angel_accZ);
 Nz=35-30*sin(Angel_accZ);
 }
 else if((0<=Angel_accY)&&(Angel_accY<=90)&&(-90<=Angel_accZ)&&(Angel_accZ<=0))
 {
 My=90-30*cos(Angel_accY);
 Ny=35-30*sin(Angel_accY);
 Mz=90+30*cos(Angel_accZ);
 Nz=35-30*sin(Angel_accZ);
 }
 else if((0<=Angel_accY)&&(Angel_accY<=90)&&(0<=Angel_accZ)&&(Angel_accZ<=90))
 {
 My=90+30*cos(Angel_accY);
 Ny=35-30*sin(Angel_accY);
 Mz=90-30*cos(Angel_accZ);
 Nz=35-30*sin(Angel_accZ);
 }
 }
 void draw(void) {
   u8g_box_frame();
   Rotate();
   u8g.setFont(u8g_font_8x13); // 6x10
   u8g.setPrintPos(0, 32); 
   u8g.print("X:");
   u8g.print(Angel_accX);
   u8g.setPrintPos(0, 48); 
   u8g.print("Y:");
   u8g.print(Angel_accY);
   u8g.setPrintPos(0, 64); 
   u8g.print("Z:");
   u8g.print(Angel_accZ);

   u8g.setPrintPos(0, 16); 
   u8g.print(right);
   u8g.setPrintPos(8, 16); 
   u8g.print("--");
   u8g.setPrintPos(24, 16);
   u8g.print(left);
   u8g.setPrintPos(32, 16); 
   u8g.print("--");
   u8g.setPrintPos(48, 16); 
   u8g.print(head);
   u8g.setPrintPos(56, 16); 
   u8g.print("--");
   u8g.setPrintPos(72, 16);
   u8g.print(back);
 }
 void loop()
 {
   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//Being three-axis acceleration and angular velocity

 //======get Acceleration values of x,y,z
    Ax=ax/16384.00;
    Ay=ay/16384.00;
    Az=az/16384.00;
    //==========there are calculated angle between thethree axes of coordinate system by acceleration
    Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14;
    Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14;
    Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;

   //==============================


   delay(10);//This is used to control the sampling rate
   //stop
   if((-5<=Angel_accX)&&(Angel_accX<=5)&&(-5<=Angel_accY)&&(Angel_accY<=5)&&(85<=Angel_accZ)&&(Angel_accZ<=90))
   {
 right=0;left=0;head=0;back=0;
   }
   //right
   else if((-15<=Angel_accX)&&(Angel_accX<=15)&&(-90<=Angel_accY)&&(Angel_accY<=-5)&&(-5<=Angel_accZ)&&(Angel_accZ<=84))
   {
 right=1;left=0;head=0;back=0;
   }
     //left
   else if((-15<=Angel_accX)&&(Angel_accX<=15)&&(-5<=Angel_accY)&&(Angel_accY<=84)&&(-5<=Angel_accZ)&&(Angel_accZ<=90))
   {
 right=0;left=1;head=0;back=0;
   }
     //head
   else if((-5<=Angel_accX)&&(Angel_accX<=90)&&(-15<=Angel_accY)&&(Angel_accY<=15)&&(-5<=Angel_accZ)&&(Angel_accZ<=84))
   {
 right=0;left=0;head=1;back=0;
   }
     //back
   else if((-90<=Angel_accX)&&(Angel_accX<=-5)&&(-15<=Angel_accY)&&(Angel_accY<=15)&&(-5<=Angel_accZ)&&(Angel_accZ<=84))
   {
 right=0;left=0;head=0;back=1;
   }

   digitalWrite(LED0, right);
   digitalWrite(LED1, left);
   digitalWrite(LED2, head);
   digitalWrite(LED3, back);


     u8g.firstPage();  
   do {
     draw();
   } while( u8g.nextPage() );
 }