//MPU6050_MyJet_V2
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define LED_PIN 13

static int16_t ax, ay, az;//加速度
static int16_t gx, gy, gz;//角速度
static long I1gx=0,I1gy=0,I1gz=0;//1階積分値
static bool blinkState = false;
static long aGx,aGy,aGz;//平均値

void setup()
{
  Wire.begin();
  Serial.begin(19200);
  accelgyro.initialize();//Initializing I2C devices
  pinMode(LED_PIN, OUTPUT);
  long sGx=0,sGy=0,sGz=0;
  int i,N=500;
  delay(300);
  for(i=0;i<N;i++)
  {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sGx+=gx;sGy+=gy;sGz+=gz;
  }
  aGx=sGx/N;aGy=sGy/N;aGz=sGz/N;
  //delay(100);
}

void loop() 
{
  int axp,ayp,azp,gxp,gyp,gzp;
  long sAx=0,sAy=0,sAz=0,sGx=0,sGy=0,sGz=0;
  int i,j,N=20;

  for(i=0;i<N;i++)
    {
      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      sAx+=ax;sAy+=ay;sAz+=az;sGx+=gx;sGy+=gy;sGz+=gz;
    }
    axp=sAx/N;ayp=sAy/N;azp=sAz/N;gxp=sGx/N;gyp=sGy/N;gzp=sGz/N;
    axp=ax;
    ayp=ay;
    azp=az;
    if(abs(gxp-aGx)>50){gxp=gxp-aGx;}
    else{gxp=0;}
    if(abs(gyp-aGy)>50){gyp=gyp-aGy;}
    else{gyp=0;}
    if(abs(gz-aGz)>50){gzp=gz-aGz;}
    else{gzp=0;}

    //I1gx+=(gxp-I1gx/500);I1gy+=(gyp-I1gy/500);I1gz+=(gzp-I1gz/500);
    I1gx+=(gxp);I1gy+=(gyp);I1gz+=(gzp);

    Serial.print(axp); Serial.print(" ");
    Serial.print(ayp); Serial.print(" ");
    Serial.print(azp); Serial.print(" ");
    Serial.print(I1gx); Serial.print(" ");
    Serial.print(I1gy); Serial.print(" ");
    Serial.print(I1gz);Serial.println(" ");
    blinkState = !blinkState;// blink LED to indicate activity
    digitalWrite(LED_PIN, blinkState);
    //delay(50);
}