from machine import Pin, I2C
import time
import mpu6050

# Initialize I2C with SCL=Pin 22, SDA=Pin 21
i2c = I2C(scl=Pin(22), sda=Pin(21))
mpu = mpu6050.accel(i2c)

def read_sensor_data():
    """Read and return formatted sensor data"""
    values = mpu.get_values()
    
    # Process accelerometer data (in g)
    accel = {
        'x': values["AcX"] / 16384.0,
        'y': values["AcY"] / 16384.0,
        'z': values["AcZ"] / 16384.0
    }
    
    # Process gyroscope data (in °/sec)
    gyro = {
        'x': values["GyX"] / 131.0,
        'y': values["GyY"] / 131.0,
        'z': values["GyZ"] / 131.0
    }
    
    return accel, gyro

def print_sensor_data(accel, gyro):
    """Print sensor data in a clean format"""
    print("\n--- MPU6050 Sensor Data ---")
    print("Accelerometer (g):")
    print(f"  X: {accel['x']:>7.3f}  Y: {accel['y']:>7.3f}  Z: {accel['z']:>7.3f}")
    print("Gyroscope (°/s):")
    print(f"  X: {gyro['x']:>7.3f}  Y: {gyro['y']:>7.3f}  Z: {gyro['z']:>7.3f}")
    print("-" * 30)

try:
    while True:
        accel_data, gyro_data = read_sensor_data()
        print_sensor_data(accel_data, gyro_data)
        time.sleep(0.2)  # Reduced delay for more frequent updates

except KeyboardInterrupt:
    print("\nProgram stopped by user")
except Exception as e:
    print(f"\nError occurred: {e}")
finally:
    print("Cleaning up and exiting")