//MPU6050_MyJet
#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 I1ax=0,I1ay=0,I1az=0,I1gx=0,I1gy=0,I1gz=0;//1階積分値
static long I2ax=0,I2ay=0,I2az=0;//2階積分値
static bool blinkState = false;
static long aAx,aAy,aAz,aGx,aGy,aGz;//平均値

void setup() 
{
  Wire.begin();
  Serial.begin(38400);
  accelgyro.initialize();//Initializing I2C devices
  pinMode(LED_PIN, OUTPUT);
  long sAx=0,sAy=0,sAz=0,sGx=0,sGy=0,sGz=0;
  int i,N=1000;
  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;
  }
  aAx=sAx/N;aAy=sAy/N;aAz=sAz/N;aGx=sGx/N;aGy=sGy/N;aGz=sGz/N;
  while (Serial.available() <= 0)
  {
    Serial.println("0,0,0,0,0,0");   // 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=50;

  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;
    int ai=256;
    aAx=(aAx*(ai-1)+axp)/ai;aAy=(aAy*(ai-1)+ayp)/ai;aAz=(aAz*(ai-1)+azp)/ai;
    axp=ax-aAx;if(abs(axp)<200){axp=0;}
    ayp=ay-aAy;if(abs(ayp)<200){ayp=0;}
    azp=az-aAz;if(abs(azp)<200){azp=0;}
    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;}

    I1ax+=(axp-I1ax/4);I1ay+=(ayp-I1ay/4);I1az+=(azp-I1az/4);
    I1gx+=(gxp-I1gx/500);I1gy+=(gyp-I1gy/500);I1gz+=(gzp-I1gz/500);
    if(abs(I1ax)<200){I1ax=0;}
    if(abs(I1ay)<200){I1ay=0;}
    if(abs(I1az)<200){I1az=0;}
    I2ax+=(I1ax/2-I2ax/20),I2ay+=(I1ay/2-I2ay/20),I2az+=(I1az/2-I2az/20);
    Serial.print(I2ax); Serial.print(",");
    Serial.print(I2ay); Serial.print(",");
    Serial.print(I2az); Serial.print(",");
    Serial.print(I1gx); Serial.print(",");
    Serial.print(I1gy); Serial.print(",");
    Serial.println(I1gz);
    blinkState = !blinkState;// blink LED to indicate activity
    digitalWrite(LED_PIN, blinkState);
    //delay(20);
    int incomingByte = 0;   // for incoming serial data
    incomingByte = Serial.read();
  }
}