pixy_i2c_emulation.py

# Esta obra está licenciada bajo la licencia MIT.
# Copyright (c) 2013-2023 OpenMV LLC. Todos los derechos reservados.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# Script de emulación I2C de Pixy
#
# Este script permite que su OpenMV Cam emule la Pixy (CMUcam5) en modo I2C.
# Tenga en cuenta que debe configurar los umbrales de color LAB a continuación para su aplicación.
#
# P4 = SCL
# P5 = SDA
#
# P7 = Servo 1
# P8 = Servo 2
#
# Parámetros de Pixy ############################################################

import math
import pyb
import csi
import struct
import time

color_code_mode = 1  # 0 == Deshabilitado, 1 == Habilitado, 2 == Solo códigos de color, 3 == Mixto

max_blocks = 1000
max_blocks_per_signature = 1000
min_block_area = 20

i2c_address = 0x54

# Servo de paneo
s0_lower_limit = 1000  # Límite inferior del ancho de pulso del servo en microsegundos.
s0_upper_limit = 2000  # Límite superior del ancho de pulso del servo en microsegundos.

# Servo de inclinación
s1_lower_limit = 1000  # Límite inferior del ancho de pulso del servo en microsegundos.
s1_upper_limit = 2000  # Límite superior del ancho de pulso del servo en microsegundos.

analog_out_enable = False  # P6 -> Salida analógica (0v - 3.3v).
analog_out_mode = 0  # 0 == posición x del blob más grande - 1 == posición y del blob más grande

# Parámetro 0 - L Mín.
# Parámetro 1 - L Máx.
# Parámetro 2 - A Mín.
# Parámetro 3 - A Máx.
# Parámetro 4 - B Mín.
# Parámetro 5 - B Máx.
# Parámetro 6 - ¿Es un umbral de código de color? (True/False).
# Parámetro 7 - ¿Habilitar umbral? (True/False).
lab_color_thresholds = [
    (0, 100, 40, 127, -128, 127, True, True),  # Umbral genérico de rojo
    (0, 100, -128, -10, -128, 127, True, True),  # Umbral genérico de verde
    (0, 0, 0, 0, 0, 0, False, False),
    (0, 0, 0, 0, 0, 0, False, False),
    (0, 0, 0, 0, 0, 0, False, False),
    (0, 0, 0, 0, 0, 0, False, False),
    (0, 0, 0, 0, 0, 0, False, False),
]

fb_pixels_threshold = 500  # número mínimo de píxeles que debe tener un blob
fb_merge_margin = 5  # qué tan cerca en píxeles pueden estar los blobs antes de fusionarse

##############################################################################

e_lab_color_thresholds = []  # umbrales habilitados
e_lab_color_code = []  # código de color habilitado
e_lab_color_signatures = []  # índices originales de umbrales habilitados
for i in range(len(lab_color_thresholds)):
    if lab_color_thresholds[i][7]:
        e_lab_color_thresholds.append(lab_color_thresholds[i][0:6])
        e_lab_color_code.append(lab_color_thresholds[i][6])
        e_lab_color_signatures.append(i + 1)

# Configuración de la cámara
csi0 = csi.CSI()
csi0.reset()
csi0.pixformat(csi.RGB565)
csi0.framesize(csi.QVGA)
csi0.snapshot(time=2000)
csi0.auto_gain(False)
csi0.auto_whitebal(False)

# Configuración del LED
red_led = pyb.LED(1)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)

red_led.off()
green_led.off()
blue_led.off()

# Configuración del DAC
dac = pyb.DAC("P6") if analog_out_enable else None

if dac:
    dac.write(0)

# Configuración del servo
min_s0_limit = min(s0_lower_limit, s0_upper_limit)
max_s0_limit = max(s0_lower_limit, s0_upper_limit)
min_s1_limit = min(s1_lower_limit, s1_upper_limit)
max_s1_limit = max(s1_lower_limit, s1_upper_limit)

s0_pan = pyb.Servo(1)  # P7
s1_tilt = pyb.Servo(2)  # P8

s0_pan.pulse_width(int((max_s0_limit - min_s0_limit) // 2))  # centro
s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2))  # centro

s0_pan_conversion_factor = (max_s0_limit - min_s0_limit) / 1000
s1_tilt_conversion_factor = (max_s1_limit - min_s1_limit) / 1000


def s0_pan_position(value):
    s0_pan.pulse_width(
        round(s0_lower_limit + (max(min(value, 1000), 0) * s0_pan_conversion_factor))
    )


def s1_tilt_position(value):
    s1_tilt.pulse_width(
        round(s1_lower_limit + (max(min(value, 1000), 0) * s1_tilt_conversion_factor))
    )


# Configuración del enlace
bus = pyb.I2C(2, pyb.I2C.SLAVE, addr=i2c_address)


def write(data):
    # Preparar primero los datos a transmitir para poder hacerlo rápido.
    out_data = []
    for i in range(0, len(data), 2):
        out_data.append(data[i : i + 2])
    # Deshabilitar las interrupciones para poder enviar todos los paquetes sin espacios.
    state = pyb.disable_irq()
    for i in range(len(out_data)):
        max_exceptions = 10
        loop = True
        while loop:
            try:
                bus.send(out_data[i], timeout=1)
                loop = False
            except OSError as error:
                if max_exceptions <= 0:
                    pyb.enable_irq(state)
                    return
                max_exceptions -= 1
    pyb.enable_irq(state)


def available():
    return 0  # No implementado ya que no hay forma de que nosotros estemos listos para recibir los datos.


def read_byte():
    return 0  # No implementado ya que no hay forma de que nosotros estemos listos para recibir los datos.


def checksum(data):
    checksum = 0
    for i in range(0, len(data), 2):
        checksum += ((data[i + 1] & 0xFF) << 8) | ((data[i + 0] & 0xFF) << 0)
    return checksum & 0xFFFF


def get_normal_signature(code):
    for i in range(len(e_lab_color_signatures)):
        if code & (1 << i):
            return e_lab_color_signatures[i]
    return 0


def to_normal_object_block_format(blob):
    temp = struct.pack(
        "<hhhhh",
        get_normal_signature(blob.code),
        blob.cx,
        blob.cy,
        blob.w,
        blob.h,
    )
    return struct.pack("<hh10s", 0xAA55, checksum(temp), temp)


def get_color_code_signature(code):
    color_code_list = []
    for i in range(len(e_lab_color_signatures)):
        if code & (1 << i):
            color_code_list.append(e_lab_color_signatures[i])
    octal = 0
    color_code_list_len = len(color_code_list) - 1
    for i in range(color_code_list_len + 1):
        octal += color_code_list[i] << (3 * (color_code_list_len - i))
    return octal


def to_color_code_object_block_format(blob):
    angle = int((blob.rotation * 180) // math.pi)
    temp = struct.pack(
        "<hhhhhh",
        get_color_code_signature(blob.code),
        blob.cx,
        blob.cy,
        blob.w,
        blob.h,
        angle,
    )
    return struct.pack("<hh12s", 0xAA56, checksum(temp), temp)


def get_signature(blob, bits):
    return (
        get_normal_signature(blob.code)
        if (bits == 1)
        else get_color_code_signature(blob.code)
    )


def to_object_block_format(blob, bits):
    return (
        to_normal_object_block_format(blob)
        if (bits == 1)
        else to_color_code_object_block_format(blob)
    )


# Código FSM
fsm_state = 0
last_byte = 0

FSM_STATE_NONE = 0
FSM_STATE_ZERO = 1
FSM_STATE_SERVO_CONTROL_0 = 2
FSM_STATE_SERVO_CONTROL_1 = 3
FSM_STATE_SERVO_CONTROL_2 = 4
FSM_STATE_SERVO_CONTROL_3 = 5
FSM_STATE_CAMERA_CONTROL = 6
FSM_STATE_LED_CONTROL_0 = 7
FSM_STATE_LED_CONTROL_1 = 8
FSM_STATE_LED_CONTROL_2 = 9


def parse_byte(byte):
    global fsm_state
    global last_byte

    if fsm_state == FSM_STATE_NONE:
        if byte == 0x00:
            fsm_state = FSM_STATE_ZERO
        else:
            fsm_state = FSM_STATE_NONE

    elif fsm_state == FSM_STATE_ZERO:
        if byte == 0xFF:
            fsm_state = FSM_STATE_SERVO_CONTROL_0
        elif byte == 0xFE:
            fsm_state = FSM_STATE_CAMERA_CONTROL
        elif byte == 0xFD:
            fsm_state = FSM_STATE_LED_CONTROL_0
        else:
            fsm_state = FSM_STATE_NONE

    elif fsm_state == FSM_STATE_SERVO_CONTROL_0:
        fsm_state = FSM_STATE_SERVO_CONTROL_1

    elif fsm_state == FSM_STATE_SERVO_CONTROL_1:
        fsm_state = FSM_STATE_SERVO_CONTROL_2
        s0_pan_position(((byte & 0xFF) << 8) | ((last_byte & 0xFF) << 0))

    elif fsm_state == FSM_STATE_SERVO_CONTROL_2:
        fsm_state = FSM_STATE_SERVO_CONTROL_3

    elif fsm_state == FSM_STATE_SERVO_CONTROL_3:
        fsm_state = FSM_STATE_NONE
        s1_tilt_position(((byte & 0xFF) << 8) | ((last_byte & 0xFF) << 0))

    elif fsm_state == FSM_STATE_CAMERA_CONTROL:
        fsm_state = FSM_STATE_NONE
        # Ignorar...

    elif fsm_state == FSM_STATE_LED_CONTROL_0:
        fsm_state = FSM_STATE_LED_CONTROL_1
        if byte & 0x80:
            red_led.on()
        else:
            red_led.off()

    elif fsm_state == FSM_STATE_LED_CONTROL_1:
        fsm_state = FSM_STATE_LED_CONTROL_2
        if byte & 0x80:
            green_led.on()
        else:
            green_led.off()

    elif fsm_state == FSM_STATE_LED_CONTROL_2:
        fsm_state = FSM_STATE_NONE
        if byte & 0x80:
            blue_led.on()
        else:
            blue_led.off()

    last_byte = byte


# Bucle principal
pri_color_code_mode = color_code_mode % 4


def bits_set(code):
    count = 0
    for i in range(7):
        count += 1 if (code & (1 << i)) else 0
    return count


def color_code(code):
    for i in range(len(e_lab_color_code)):
        if code & (1 << i):
            return e_lab_color_code[i]
    return False


def fb_merge_cb(blob0, blob1):
    if not pri_color_code_mode:
        return blob0.code == blob1.code
    else:
        return (
            True
            if (blob0.code == blob1.code)
            else (color_code(blob0.code) and color_code(blob1.code))
        )


def blob_filter(blob):
    if pri_color_code_mode == 0:
        return True
    elif pri_color_code_mode == 1:  # códigos de color con dos o más colores, o normales
        return (bits_set(blob.code) > 1) or (not color_code(blob.code))
    elif pri_color_code_mode == 2:  # solo códigos de color con dos o más colores
        return bits_set(blob.code) > 1
    elif pri_color_code_mode == 3:
        return True


clock = time.clock()
while True:
    clock.tick()
    img = csi0.snapshot()
    blobs = list(
        filter(
            blob_filter,
            img.find_blobs(
                e_lab_color_thresholds,
                area_threshold=min_block_area,
                pixels_threshold=fb_pixels_threshold,
                merge=True,
                margin=fb_merge_margin,
                merge_cb=fb_merge_cb,
            ),
        )
    )

    # Transmitir blobs
    if blobs and (max_blocks > 0) and (max_blocks_per_signature > 0):  # nuevo fotograma
        dat_buf = struct.pack("<h", 0xAA55)
        sig_map = {}
        first_b = False

        for blob in sorted(blobs, key=lambda x: x.area, reverse=True)[0:max_blocks]:
            bits = bits_set(blob.code)
            sign = get_signature(blob, bits)

            if not sign in sig_map:
                sig_map[sign] = 1
            else:
                sig_map[sign] += 1

            if sig_map[sign] <= max_blocks_per_signature:
                dat_buf += to_object_block_format(blob, bits)
                img.draw_detection(blob)

            if dac and not first_b:
                x_scale = 255 / (img.width() - 1)
                y_scale = 255 / (img.height() - 1)
                dac.write(
                    round(
                        (blob.y * y_scale)
                        if analog_out_mode
                        else (blob.x * x_scale)
                    )
                )
                first_b = True

        dat_buf += struct.pack("<h", 0x0000)
        write(dat_buf)  # escribir todos los datos en un solo paquete...

    else:  # no se encontró nada
        write(struct.pack("<h", 0x0000))

        if dac:
            dac.write(0)

    # Analizar comandos
    for i in range(available()):
        parse_byte(read_byte())

    num_blobs = min(len(blobs), max_blocks)
    print("%d blob(s) found - FPS %f" % (num_blobs, clock.fps()))