import time
from machine import I2C, Pin

# MPU6050 register addresses
MPU6050_ADDR = 0x68
GYRO_XOUT_H = 0x43
GYRO_XOUT_L = 0x44
GYRO_YOUT_H = 0x45
GYRO_YOUT_L = 0x46
GYRO_ZOUT_H = 0x47
GYRO_ZOUT_L = 0x48
ACCEL_XOUT_H = 0x3B
ACCEL_XOUT_L = 0x3C
ACCEL_YOUT_H = 0x3D
ACCEL_YOUT_L = 0x3E
ACCEL_ZOUT_H = 0x3F
ACCEL_ZOUT_L = 0x40

# Initialize I2C interface
i2c = I2C(0, sda=Pin(20), scl=Pin(21), freq=400000)

# Lists to store gyroscope and accelerometer data
gyro_data = []
accel_data = []

# Read gyroscope data
def read_gyro():
    data = i2c.readfrom_mem(MPU6050_ADDR, GYRO_XOUT_H, 6)
    x_gyro = (data[0] << 8 | data[1]) / 131.0
    y_gyro = (data[2] << 8 | data[3]) / 131.0
    z_gyro = (data[4] << 8 | data[5]) / 131.0
    return x_gyro, y_gyro, z_gyro

# Read accelerometer data
def read_accel():
    data = i2c.readfrom_mem(MPU6050_ADDR, ACCEL_XOUT_H, 6)
    x_accel = (data[0] << 8 | data[1]) / 16384.0
    y_accel = (data[2] << 8 | data[3]) / 16384.0
    z_accel = (data[4] << 8 | data[5]) / 16384.0
    return x_accel, y_accel, z_accel

# Main loop
while True:
    x_gyro, y_gyro, z_gyro = read_gyro()
    x_accel, y_accel, z_accel = read_accel()
    
    gyro_data.append((x_gyro, y_gyro, z_gyro))
    accel_data.append((x_accel, y_accel, z_accel))
    
    print("Gyro: X={:.2f} deg/s, Y={:.2f} deg/s, Z={:.2f} deg/s".format(x_gyro, y_gyro, z_gyro))
    print("Accel: X={:.2f} g, Y={:.2f} g, Z={:.2f} g".format(x_accel, y_accel, z_accel))
    
    time.sleep(1)

# Optionally, print the collected data lists after a certain period of time
print("Collected Gyro Data:", gyro_data)
print("Collected Accel Data:", accel_data)