Untitled

import time
import board
import busio
import adafruit_vl53l0x
from adafruit_tca9548a import TCA9548A
from adafruit_pca9685 import PCA9685
from adafruit_motor import servo
import statistics
import sys
import tty
import termios

# Initialize I2C buses
i2c_mux = busio.I2C(board.SCL, board.SDA)  # For multiplexer
i2c_servo = busio.I2C(board.SCL, board.SDA)  # For servo controller

print("Initializing I2C multiplexer...")
try:
    # Initialize multiplexer
    mux = TCA9548A(i2c_mux)
except Exception as e:
    print(f"Failed to initialize multiplexer: {str(e)}")
    sys.exit(1)

# Initialize PCA9685 directly on I2C bus (address 0x40)
try:
    pca = PCA9685(i2c_servo, address=0x40)
    pca.frequency = 50  # Standard servo frequency
except Exception as e:
    print(f"Failed to initialize PCA9685: {str(e)}")
    sys.exit(1)

# Initialize servo
servo_motor = servo.Servo(pca.channels[0], min_pulse=1300, max_pulse=3600)
servo_motor.angle = 0  # Set to home position

# Initialize sensors with proper error handling
print("Initializing sensors...")
sensors = {}
for channel in range(8):
    try:
        print(f"Initializing sensor on channel {channel}...")
        # Select the channel by setting its bit in the channel state
        mux.i2c_device.write(bytes([1 << channel]))
        time.sleep(0.1)  # Give the multiplexer time to switch
        
        # Initialize the sensor
        sensor = adafruit_vl53l0x.VL53L0X(mux[channel])
        
        # Optional: You can set a longer timing budget for more accurate readings
        sensor.measurement_timing_budget = 200000  # 200ms
        
        sensors[channel] = sensor
        print(f"Successfully initialized sensor on channel {channel}")
        
    except ValueError as ve:
        print(f"No sensor found on channel {channel}: {str(ve)}")
    except Exception as e:
        print(f"Error initializing sensor on channel {channel}: {str(e)}")
    
    time.sleep(0.1)  # Brief pause between sensor initializations

if not sensors:
    print("No sensors were successfully initialized. Exiting...")
    sys.exit(1)

print(f"Successfully initialized {len(sensors)} sensors")