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

MPU6050 mpu;

void setup() 
{
  Serial.begin(115200);

  Serial.println("Initialize MPU6050");
  mpu.initialize();
  if (mpu.testConnection()) {
    Serial.println("Connection Successful!");
  } else {
    Serial.println("Connection Failed!");
    while (1); // Stop running the program if connection is failed.
  }
}

void loop()
{
  // Read raw accelerometer and gyroscope data
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Calculate normalized (scaled) accelerometer values
  float normAccelX = ax / 16384.0; // 16384 is the sensitivity scale
  float normAccelY = ay / 16384.0;
  float normAccelZ = az / 16384.0;

  Serial.print(" Xraw = ");
  Serial.print(ax);
  Serial.print(" Yraw = ");
  Serial.print(ay);
  Serial.print(" Zraw = ");
  Serial.print(az);
  Serial.print(" Xnorm = ");
  Serial.print(normAccelX);
  Serial.print(" Ynorm = ");
  Serial.print(normAccelY);
  Serial.print(" Znorm = ");
  Serial.println(normAccelZ);

  delay(1000);
}