pixy_spi_emulation.py

# Questo lavoro è concesso in licenza MIT.
# Copyright (c) 2013-2023 OpenMV LLC. Tutti i diritti riservati.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# Script di emulazione Pixy SPI
#
# Questo script consente alla tua OpenMV Cam di emulare la Pixy (CMUcam5) in modalità SPI.
# Nota che è necessario configurare le soglie di colore LAB qui sotto per la tua applicazione.
#
# P0 = MOSI
# P1 = MISO
# P2 = SCLK
# P3 = SS
#
# P7 = Servo 1
# P8 = Servo 2
#
# Nota: assicurati che il tuo Arduino, ecc. stia eseguendo il suo codice SPI prima che
# la OpenMV Cam inizi a funzionare. Inoltre, a volte la OpenMV Cam non riesce
# a sincronizzarsi con il flusso di dati SPI dell'Arduino. Riavvia semplicemente la
# OpenMV Cam quando ciò accade. La OpenMV Cam dovrebbe sincronizzarsi entro
# 1-9 tentativi. Altrimenti usa la versione seriale o I2C di questo script.

import math
import pyb
import csi
import struct
import time

# Parametri Pixy ############################################################
color_code_mode = 1  # 0 == Disabilitato, 1 == Abilitato, 2 == Solo codici colore, 3 == Misto

max_blocks = 1000
max_blocks_per_signature = 1000
min_block_area = 20

# Servo Pan
s0_lower_limit = 1000  # Limite inferiore della larghezza di impulso del servo in microsecondi.
s0_upper_limit = 2000  # Limite superiore della larghezza di impulso del servo in microsecondi.

# Servo Tilt
s1_lower_limit = 1000  # Limite inferiore della larghezza di impulso del servo in microsecondi.
s1_upper_limit = 2000  # Limite superiore della larghezza di impulso del servo in microsecondi.

analog_out_enable = False  # P6 -> Uscita analogica (0v - 3.3v).
analog_out_mode = 0  # 0 == posizione x del blob più grande - 1 == posizione y del blob più grande

# Parametro 0 - L Min.
# Parametro 1 - L Max.
# Parametro 2 - A Min.
# Parametro 3 - A Max.
# Parametro 4 - B Min.
# Parametro 5 - B Max.
# Parametro 6 - È una soglia di codice colore? (True/False).
# Parametro 7 - Abilita soglia? (True/False).
lab_color_thresholds = [
    (0, 100, 40, 127, -128, 127, True, True),  # Soglia generica per il rosso
    (0, 100, -128, -10, -128, 127, True, True),  # Soglia generica per il 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  # numero minimo di pixel che devono essere presenti in un blob
fb_merge_margin = 5  # quanto vicini possono essere i blob, in termini di pixel, prima di unirsi

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

e_lab_color_thresholds = []  # soglie abilitate
e_lab_color_code = []  # codice colore abilitato
e_lab_color_signatures = []  # indici originali delle soglie abilitate
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)

# Configurazione della fotocamera
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)

# Configurazione 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()

# Configurazione DAC
dac = pyb.DAC("P6") if analog_out_enable else None

if dac:
    dac.write(0)

# Configurazione 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))
    )


# Configurazione del collegamento
bus = pyb.SPI(2, pyb.SPI.SLAVE, polarity=0, phase=0, bits=16)
while True:
    try:
        sync_bytes = bus.recv(2, timeout=10)
        if (sync_bytes[0] == 0x00) and (sync_bytes[1] == 0x5A):
            break
    except OSError as error:
        pass

    bus.deinit()
    bus.init(pyb.SPI.SLAVE, polarity=0, phase=0, bits=16)


def write(data):
    max_exceptions = 10
    loop = True
    while loop:
        try:
            bus.send(data, timeout=10)
            loop = False
        except OSError as error:
            if max_exceptions <= 0:
                return
            max_exceptions -= 1


def available():
    return 0  # Non implementato poiché non c'è modo per noi di essere pronti a ricevere i dati.


def read_byte():
    return 0  # Non implementato poiché non c'è modo per noi di essere pronti a ricevere i dati.


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)
    )


# Codice 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
        # Ignora...

    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


# Ciclo principale

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:  # codici colore con due o più colori o normali
        return (bits_set(blob.code) > 1) or (not color_code(blob.code))
    elif pri_color_code_mode == 2:  # solo codici colore con due o più colori
        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,
            ),
        )
    )

    # Trasmetti blob
    if blobs and (max_blocks > 0) and (max_blocks_per_signature > 0):  # nuovo fotogramma
        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)  # scrivi tutti i dati in un unico pacchetto...

    else:  # nessun risultato trovato
        write(struct.pack("<h", 0x0000))

        if dac:
            dac.write(0)

    # Analizza i comandi
    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()))