import serial
import time

# Configure the serial port for your Raspberry Pi and Maestro
# Replace '/dev/ttyACM0' with the correct serial port for your setup
# And 9600 with the baud rate configured for the Maestro
ser = serial.Serial('/dev/ttyACM0', 9600, timeout=1)

def get_maestro_errors():
    # Pololu Maestro "Get Errors" command (Compact Protocol)
    # Command: 0xA1
    ser.write(bytearray([0xA1]))
    # Read 2 bytes for the error flags
    error_bytes = ser.read(2)
    if len(error_bytes) == 2:
        # Combine bytes to form the 16-bit error flags
        error_flags = error_bytes[0] + (error_bytes[1] << 8)
        return error_flags
    else:
        return None

def clear_maestro_errors():
    # Pololu Maestro "Clear Errors" command (Compact Protocol)
    # Command: 0xA2
    ser.write(bytearray([0xA2]))

if __name__ == "__main__":
    try:
        errors = get_maestro_errors()
        if errors is not None:
            print(f"Maestro Error Flags: 0x{errors:04X}")
            # You can then interpret the individual bits based on the Maestro User's Guide
            if errors & 0x0001:
                print("  - Serial Error")
            if errors & 0x0002:
                print("  - Script Error")
            # ... and so on for other error flags
            
            # Clear errors after reading
            clear_maestro_errors()
            print("Maestro errors cleared.")
        else:
            print("Failed to read Maestro errors.")

    except serial.SerialException as e:
        print(f"Serial communication error: {e}")
    finally:
        ser.close()