#include "Adafruit_MPU6050.h"
#include "Adafruit_Sensor.h"
#include "Wire.h"

Adafruit_MPU6050 MPU;

void setup() {

  Serial.begin(115200);
  Serial.println("MPU6050 Sensor test! ");

  if(!MPU.begin()){
    Serial.println("Failed to connect to the Sensor!!");
    while(1) delay(10);
  }

  Serial.println("MPU6050 Sensor Connected!!");

  MPU.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");

  switch(MPU.getAccelerometerRange()){

    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;

  }

  MPU.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyroscope range set to : ");

  switch(MPU.getGyroRange()){

    case MPU6050_RANGE_250_DEG:
      Serial.println("+-250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+-500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+-1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+-2000 deg/s");
      break;

  }

  MPU.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to : ");

  switch(MPU.getFilterBandwidth()){

    case MPU6050_BAND_260_HZ:
      Serial.println("260 HZ"); 
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 HZ"); 
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 HZ"); 
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 HZ"); 
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 HZ"); 
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 HZ"); 
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 HZ"); 
      break;
  }

  Serial.println("");
  delay(100);
}

void loop() {

  sensors_event_t ACCEL, GYRO, TEMP;
  MPU.getEvent(&ACCEL, &GYRO, &TEMP);

  // Print the Values
  Serial.println("===========  ACCELERATION ==========");
  Serial.print("Acceleration [X] : ");
  Serial.print(ACCEL.acceleration.x);
  Serial.print(" m/s^2          ");

  Serial.print("Acceleration [Y] : ");
  Serial.print(ACCEL.acceleration.y);
  Serial.print(" m/s^2          ");

  Serial.print("Acceleration [Z] : ");
  Serial.print(ACCEL.acceleration.z);
  Serial.println(" m/s^2          ");

  Serial.println("===========  GYROSCOPE ==========");
  Serial.print("Rotation [X] : ");
  Serial.print(GYRO.gyro.x);
  Serial.print(" rad/s          ");

  Serial.print("Rotation [Y] : ");
  Serial.print(GYRO.gyro.y);
  Serial.print(" rad/s          ");

  Serial.print("Rotation [Z] : ");
  Serial.print(GYRO.gyro.z);
  Serial.println(" rad/s          ");

  Serial.println("===========  TEMPERATURE ==========");
  Serial.print("Temperature : ");
  Serial.print(TEMP.temperature);
  Serial.println(" degC          ");

  Serial.println("");
  delay(500);
}