#include <Wire.h>

const int i = 0x68; 

int16_t accelX, accelY, accelZ;
int16_t gyroX, gyroY, gyroZ;

void setup() {
  Serial1.begin(115200);   
  delay(1000);

  Wire.setSDA(4);   
  Wire.setSCL(5);   
  Wire.begin();

  // Wake up MPU6050
  Wire.beginTransmission(i);
  Wire.write(0x6B);  
  Wire.write(0);     
  Wire.endTransmission(true);

  Serial1.println("MPU6050 Ready");
}

void loop() {
  Wire.beginTransmission(i);
  Wire.write(0x3B);  
  Wire.endTransmission(false);
  Wire.requestFrom(i, 14, true);

  accelX = Wire.read() << 8 | Wire.read();
  accelY = Wire.read() << 8 | Wire.read();
  accelZ = Wire.read() << 8 | Wire.read();

  Wire.read(); Wire.read(); 

  gyroX = Wire.read() << 8 | Wire.read();
  gyroY = Wire.read() << 8 | Wire.read();
  gyroZ = Wire.read() << 8 | Wire.read();

  Serial1.println("---- Accelerometer ----");
  Serial1.print("X: "); Serial1.println(accelX);
  Serial1.print("Y: "); Serial1.println(accelY);
  Serial1.print("Z: "); Serial1.println(accelZ);

  Serial1.println("---- Gyroscope ----");
  Serial1.print("X: "); Serial1.println(gyroX);
  Serial1.print("Y: "); Serial1.println(gyroY);
  Serial1.print("Z: "); Serial1.println(gyroZ);

  Serial1.println("-----------------------");

  delay(1000);
}