import machine
import time

# I2C Address of the MPU6050
MPU6050_I2C_ADDR = 0x68

# MPU6050 Register Addresses
MPU6050_REG_ACCEL_XOUT_H = 0x3B
MPU6050_REG_PWR_MGMT_1 = 0x6B

# Initialize I2C
i2c = machine.I2C(0, scl=machine.Pin(9), sda=machine.Pin(8), freq=400000)

# Initialize MPU6050
i2c.writeto_mem(MPU6050_I2C_ADDR, MPU6050_REG_PWR_MGMT_1, bytes([0]))

def read_acceleration():
    # Read raw accelerometer data
    data = i2c.readfrom_mem(MPU6050_I2C_ADDR, MPU6050_REG_ACCEL_XOUT_H, 6)

    # Convert raw data to signed 16-bit values
    accel_x_raw = (data[0] << 8 | data[1])
    accel_y_raw = (data[2] << 8 | data[3])
    accel_z_raw = (data[4] << 8 | data[5])

    # Handle sign extension for negative values
    accel_x = accel_x_raw if accel_x_raw < 0x8000 else accel_x_raw - 0x10000
    accel_y = accel_y_raw if accel_y_raw < 0x8000 else accel_y_raw - 0x10000
    accel_z = accel_z_raw if accel_z_raw < 0x8000 else accel_z_raw - 0x10000

    # Convert to acceleration in terms of g
    accel_x /= 16384.0
    accel_y /= 16384.0
    accel_z /= 16384.0

    return accel_x, accel_y, accel_z

while True:
    accel_x, accel_y, accel_z = read_acceleration()
    print("Acceleration X: {:.2f}g, Y: {:.2f}g, Z: {:.2f}g".format(accel_x, accel_y, accel_z))
    time.sleep(1)