//MPU6050_MyJet_V3
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define LED_PIN 13
#define SW_PIN 8

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

void setup()
{
  Wire.begin();
  Serial.begin(38400);
  accelgyro.initialize();//Initializing I2C devices
  pinMode(LED_PIN, OUTPUT);
  pinMode(SW_PIN, INPUT);
  long sGx=10,sGy=10,sGz=10;
  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;
  while (Serial.available() <= 0) 
  {
    Serial.println("0,0,0,0,0,0,1");   // send an initial string
    delay(300);
  }
}

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;

  int SW=digitalRead(SW_PIN);
  if(SW==LOW)
  {
    if(gSW==HIGH)
    {
      I1gx=0;I1gy=0;I1gz=0;
    }
    gSW=LOW;
  }
  else
  {gSW=HIGH;}

  if (Serial.available() > 0)
  {
    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);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.print(",");
    Serial.println(gSW);
    blinkState = !blinkState;// blink LED to indicate activity
    if(gSW==LOW){blinkState=HIGH;}
    digitalWrite(LED_PIN, blinkState);
  }
  delay(100);
}