#include <Wire.h>
#include <MPU6050.h>
#include <I2Cdev.h>

MPU6050 accelgyro; // construct MPU6050 object using default object
int16_t ax, ay, az; // acceleration of 3 axes
int16_t gx, gy, gz; // gyroscope of 3 axes
#define LED_PIN 13 // number of the LED pin
bool blinkState = false; // state of LED

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("Initalizing I2C devices...");
  Wire.begin(); // initialize I2C
  accelgyro.initialize(); // initiaize MPU6050
  Serial.println("Testing device connections");
  // verify connection
  if(accelgyro.testConnection()){
    Serial.println("MPU6050 has successfully connected");
  } else {
    Serial.println("MPU6050 has failed to connect");
    while(1);
  }

  // calibrate gravity acceleration
  accelgyro.setXAccelOffset(-1200);
  accelgyro.setYAccelOffset(-2500);
  accelgyro.setZAccelOffset(1988);
  Serial.print("X.Y.Z offset :\t");
  Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t");
  Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t");
  Serial.print(accelgyro.getZAccelOffset()); Serial.print("\n");
  
  // initialize pin
  pinMode(LED_PIN, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  // read raw accel/gyro measurements of device
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // display accel/gyro x/y/z values
  Serial.print("a/g:\t");
  Serial.print(ax); Serial.print("\t");
  Serial.print(ay); Serial.print("\t");
  Serial.print(az); Serial.print("\t");
  Serial.print(gx); Serial.print("\t\t");
  Serial.print(gy); Serial.print("\t\t");
  Serial.println(gz);

  // convert acceleration unit to g and gyro unit to dps (degree per second) according to sensitivity
  Serial.print("a/g:\t");
  Serial.print((float)ax / 16384); Serial.print("g\t");
  Serial.print((float)ay / 16384); Serial.print("g\t");
  Serial.print((float)az / 16384); Serial.print("g\t");
  Serial.print((float)gx / 131); Serial.print("d/s  \t");
  Serial.print((float)gy / 131); Serial.print("d/s  \t");
  Serial.print((float)gz / 131); Serial.print("d/s  \t");
  delay(300);

  // blink LED to indicate activity
  blinkState = !blinkState;
  digitalWrite(LED_PIN, blinkState);
}