import machine
import utime

# 1. Initialize I2C communication
# GP0 is SDA, GP1 is SCL
i2c = machine.I2C(0, sda=machine.Pin(0), scl=machine.Pin(1), freq=400000)

# MPU-6050 I2C address is usually 0x68
MPU_ADDR = 0x68

def init_mpu():
    # Wake up the MPU-6050 (it starts in sleep mode)
    i2c.writeto_mem(MPU_ADDR, 0x6B, b'\x00')
    utime.sleep_ms(100)
    
    # Set Accelerometer to ±16g (The "Badminton Mode")
    # 0x1C is the ACCEL_CONFIG register. 0x18 sets it to 16g.
    i2c.writeto_mem(MPU_ADDR, 0x1C, b'\x18')

def read_raw_data(addr):
    # Read 6 bytes of data (High/Low for X, Y, Z)
    data = i2c.readfrom_mem(MPU_ADDR, addr, 6)
    
    # Convert bytes to 16-bit integers
    x = (data[0] << 8) | data[1]
    y = (data[2] << 8) | data[3]
    z = (data[4] << 8) | data[5]
    
    # Handle negative numbers (Two's Complement)
    if x > 32767: x -= 65536
    if y > 32767: y -= 65536
    if z > 32767: z -= 65536
    return x, y, z

# --- Main Program ---
init_mpu()
print("MPU-6050 Initialized at ±16g range.")

while True:
    accel_x, accel_y, accel_z = read_raw_data(0x3B)
    
    # Convert raw data to G-force (for 16g range, divisor is 2048)
    g_x = accel_x / 2048.0
    g_y = accel_y / 2048.0
    g_z = accel_z / 2048.0
    
    print(f"X: {g_x:.2f}g | Y: {g_y:.2f}g | Z: {g_z:.2f}g")
    
    utime.sleep_ms(100)