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

// Initialize MPU6050
MPU6050 mpu;

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

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();

  // Check if MPU6050 connection was successful
  if (mpu.testConnection()) {
    Serial.println("MPU6050 connection successful");
  } else {
    Serial.println("MPU6050 connection failed");
    while (1); // Stop here if connection failed
  }
}

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

  // Convert raw values to meaningful units
  float accelX = (float)ax / 16384.0; // 16384 LSB per g
  float accelY = (float)ay / 16384.0;
  float accelZ = (float)az / 16384.0;
  float gyroX = (float)gx / 131.0;    // 131 LSB per degree/s
  float gyroY = (float)gy / 131.0;
  float gyroZ = (float)gz / 131.0;

  // Print sensor data to serial monitor
  Serial.print("Accelerometer (g): ");
  Serial.print(accelX);
  Serial.print(", ");
  Serial.print(accelY);
  Serial.print(", ");
  Serial.println(accelZ);

  Serial.print("Gyroscope (degrees/s): ");
  Serial.print(gyroX);
  Serial.print(", ");
  Serial.print(gyroY);
  Serial.print(", ");
  Serial.println(gyroZ);

  Serial.println();

  delay(500); // Delay between readings
}