from machine import Pin, I2C
import time

# MPU6050 I2C address
MPU6050_ADDR = 0x68

# MPU6050 Registers
PWR_MGMT_1   = 0x6B
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H  = 0x43

# Initialize I2C (example pins for ESP32: scl=22, sda=21)
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)

# Wake up MPU6050
i2c.writeto_mem(MPU6050_ADDR, PWR_MGMT_1, b'\x00')

def read_raw_data(addr):
    # Read two bytes from register
    high = i2c.readfrom_mem(MPU6050_ADDR, addr, 1)
    low  = i2c.readfrom_mem(MPU6050_ADDR, addr+1, 1)
    value = (high[0] << 8) | low[0]
    if value > 32768:   # Convert to signed
        value -= 65536
    return value

while True:
    # Accelerometer raw data
    acc_x = read_raw_data(ACCEL_XOUT_H)
    acc_y = read_raw_data(ACCEL_XOUT_H+2)
    acc_z = read_raw_data(ACCEL_XOUT_H+4)

    # Gyroscope raw data
    gyro_x = read_raw_data(GYRO_XOUT_H)
    gyro_y = read_raw_data(GYRO_XOUT_H+2)
    gyro_z = read_raw_data(GYRO_XOUT_H+4)

    # Convert to 'g' and 'deg/s'
    Ax = acc_x/16384.0
    Ay = acc_y/16384.0
    Az = acc_z/16384.0

    Gx = gyro_x/131.0
    Gy = gyro_y/131.0
    Gz = gyro_z/131.0

    print("Accel: X=%.2f g, Y=%.2f g, Z=%.2f g" % (Ax, Ay, Az))
    print("Gyro : X=%.2f °/s, Y=%.2f °/s, Z=%.2f °/s" % (Gx, Gy, Gz))
    print("-----")
    time.sleep(0.5)