from machine import I2C, Pin
import time

# Pin assignments for ESP32
scl_pin = 22
sda_pin = 21

# I2C Initialization
i2c = I2C(scl=Pin(scl_pin), sda=Pin(sda_pin), freq=400000)

# MPU-6050 I2C address (could be 0x68 or 0x69, depending on the device)
mpu6050_addr = 0x68

# MPU-6050 registers
mpu6050_pwr_mgmt_1 = 0x6B
mpu6050_acel_xout_h = 0x3B

def mpu6050_init():
    # Wake up the MPU-6050 since it starts in sleep mode
    i2c.writeto_mem(mpu6050_addr, mpu6050_pwr_mgmt_1, bytearray([0]))

def read_mpu6050_accel():
    # Read 6 bytes of acceleration data
    accel_data = i2c.readfrom_mem(mpu6050_addr, mpu6050_acel_xout_h, 6)
    # Convert the byte data to word values
    accel_x = (accel_data[0] << 8) | accel_data[1]
    accel_y = (accel_data[2] << 8) | accel_data[3]
    accel_z = (accel_data[4] << 8) | accel_data[5]
    
    # Handle signed values
    if accel_x > 32767: accel_x -= 65536
    if accel_y > 32767: accel_y -= 65536
    if accel_z > 32767: accel_z -= 65536
    
    return (accel_x, accel_y, accel_z)

def read_mpu6050_gyro():
    # Read 6 bytes of gyroscope data
    gyro_data = i2c.readfrom_mem(mpu6050_addr, 0x43, 6)
    # Convert the byte data to word values
    gyro_x = (gyro_data[0] << 8) | gyro_data[1]
    gyro_y = (gyro_data[2] << 8) | gyro_data[3]
    gyro_z = (gyro_data[4] << 8) | gyro_data[5]
    
    # Handle signed values
    if gyro_x > 32767: gyro_x -= 65536
    if gyro_y > 32767: gyro_y -= 65536
    if gyro_z > 32767: gyro_z -= 65536
    
    return (gyro_x, gyro_y, gyro_z)


mpu6050_init()

while True:
    accel_x, accel_y, accel_z = read_mpu6050_accel()
    gyro_x, gyro_y, gyro_z = read_mpu6050_gyro()
    
    print("Accel X:", accel_x, "Accel Y:", accel_y, "Accel Z:", accel_z)
    print("Gyro X:", gyro_x, "Gyro Y:", gyro_y, "Gyro Z:", gyro_z)
    
    time.sleep(1)