from hcsr04 import HCSR04
from imu import MPU6050
from time import sleep
from machine import Pin, I2C

# Initialize ultrasonic sensor
sensor = HCSR04(trigger_pin=2, echo_pin=3, echo_timeout_us=10000)

# Initialize gyroscope
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
imu = MPU6050(i2c)

# Initialize I2C for other devices
sdaPIN = machine.Pin(0)
sclPIN = machine.Pin(1)
i2c = machine.I2C(0, sda=sdaPIN, scl=sclPIN, freq=400000)

# Scan for I2C devices
devices = i2c.scan()
if len(devices) == 0:
    print("No I2C device!")
else:
    print('I2C devices found:', len(devices))
    for device in devices:
        print("At address:", hex(device))

while True:
    # Ultrasonic sensor
    distance = sensor.distance_cm()
    print('Distance:', distance, 'cm')

    # Gyroscope
    ax = round(imu.accel.x, 2)
    ay = round(imu.accel.y, 2)
    az = round(imu.accel.z, 2)
    gx = round(imu.gyro.x)
    gy = round(imu.gyro.y)
    gz = round(imu.gyro.z)
    tem = round(imu.temperature, 2)
    print("ax", ax, "\t", "ay", ay, "\t", "az", az, "\t", "gx", gx, "\t", "gy", gy, "\t", "gz", gz, "\t", "Temperature", tem, "        ", end="\r")

    # Sleep to avoid excessive CPU usage
    sleep(0.2)