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

// Create an instance of the MPU6050 class
MPU6050 mpu;

// Define the I2C address for MPU6050 (default is 0x68)
const int MPU_ADDR = 0x68;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Initialize I2C communication
  Wire.begin();

  // Initialize MPU6050
  mpu.initialize();
  
  // Check if the MPU6050 is connected
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  Serial.println("MPU6050 initialized successfully");
}

void loop() {
  // Variables to hold accelerometer and gyroscope data
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

  // Read accelerometer and gyroscope data
  mpu.getAcceleration(&ax, &ay, &az);
  mpu.getRotation(&gx, &gy, &gz);

  // Print the accelerometer data to Serial Monitor
  Serial.print("aX: "); Serial.print(ax); Serial.print(" ");
  Serial.print("aY: "); Serial.print(ay); Serial.print(" ");
  Serial.print("aZ: "); Serial.print(az); Serial.print(" ");
  Serial.print("gX: "); Serial.print(gx); Serial.print(" ");
  Serial.print("gY: "); Serial.print(gy); Serial.print(" ");
  Serial.print("gZ: "); Serial.println(gz);

  // Wait for a bit before reading again
  delay(500);
}