from machine import Pin, SoftI2C
import time
import struct

# Initialisation du SoftI2C
i2c = SoftI2C(scl=Pin(22), sda=Pin(21), freq=400000)

# Adresse par défaut du MPU6050
MPU6050_ADDR = 0x68

# Initialisation du MPU6050
def init_mpu6050():
    # Réinitialisation du MPU6050
    i2c.writeto_mem(MPU6050_ADDR, 0x6B, b'\x00')  # Mettre en mode normal

def read_raw_data(addr):
    # Lire 2 octets de données à partir de l'adresse spécifiée
    high = i2c.readfrom_mem(MPU6050_ADDR, addr, 1)
    low = i2c.readfrom_mem(MPU6050_ADDR, addr + 1, 1)
    return struct.unpack('>H', high + low)[0]

# Initialisation du MPU6050
init_mpu6050()

while True:
    # Lire les valeurs de l'accéléromètre (X, Y, Z)
    accel_x = read_raw_data(0x3B)
    accel_y = read_raw_data(0x3D)
    accel_z = read_raw_data(0x3F)

    # Lire les valeurs du gyroscope (X, Y, Z)
    gyro_x = read_raw_data(0x43)
    gyro_y = read_raw_data(0x45)
    gyro_z = read_raw_data(0x47)

    print("Accéléromètre (X, Y, Z):", accel_x, accel_y, accel_z)
    print("Gyroscope (X, Y, Z):", gyro_x, gyro_y, gyro_z)

    time.sleep(1)