#include <Wire.h>
#include <Adafruit_MPU6050.h>


Adafruit_MPU6050 mpu;

void setup() {
  Serial.begin(115200);
  // Initialize I2C communication
  Wire.begin(21, 22);  // SDA = 35, SCL = 34
  
  // Try to initialize the MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  // Set accelerometer range to +/- 4G
  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
  // Set gyroscope range to +/- 250 degrees per second
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  // Set the filter bandwidth to 21 Hz
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Give some time to calibrate
  delay(100);
}

void loop() {
  // Get new sensor events with the readings
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Print out the accelerometer data
  Serial.print("Accel X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  // Print out the gyroscope data
  Serial.print("Gyro X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  // Print out the temperature data
  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" C");

  // Delay for a bit to make the output more readable
  delay(500);
}