pixy_i2c_emulation.py

# Dieses Werk ist unter der MIT-Lizenz lizenziert.
# Copyright (c) 2013-2023 OpenMV LLC. Alle Rechte vorbehalten.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# Pixy I2C Emulationsskript
#
# Dieses Skript ermöglicht es Ihrer OpenMV Cam, die Pixy (CMUcam5) im I2C-Modus zu emulieren.
# Beachten Sie, dass Sie die LAB-Farbschwellenwerte unten für Ihre Anwendung einstellen müssen.
#
# P4 = SCL
# P5 = SDA
#
# P7 = Servo 1
# P8 = Servo 2
#
# Pixy-Parameter ############################################################

import math
import pyb
import csi
import struct
import time

color_code_mode = 1  # 0 == Deaktiviert, 1 == Aktiviert, 2 == Nur Farbcodes, 3 == Gemischt

max_blocks = 1000
max_blocks_per_signature = 1000
min_block_area = 20

i2c_address = 0x54

# Schwenkservo (Pan)
s0_lower_limit = 1000  # Untere Grenze der Servo-Pulsbreite in Mikrosekunden.
s0_upper_limit = 2000  # Obere Grenze der Servo-Pulsbreite in Mikrosekunden.

# Neigeservo (Tilt)
s1_lower_limit = 1000  # Untere Grenze der Servo-Pulsbreite in Mikrosekunden.
s1_upper_limit = 2000  # Obere Grenze der Servo-Pulsbreite in Mikrosekunden.

analog_out_enable = False  # P6 -> Analog Out (0v - 3.3v).
analog_out_mode = 0  # 0 == x-Position des größten Blobs - 1 == y-Position des größten Blobs

# Parameter 0 - L Min.
# Parameter 1 - L Max.
# Parameter 2 - A Min.
# Parameter 3 - A Max.
# Parameter 4 - B Min.
# Parameter 5 - B Max.
# Parameter 6 - Ist Farbcode-Schwellenwert? (True/False).
# Parameter 7 - Schwellenwert aktivieren? (True/False).
lab_color_thresholds = [
    (0, 100, 40, 127, -128, 127, True, True),  # Allgemeiner Rot-Schwellenwert
    (0, 100, -128, -10, -128, 127, True, True),  # Allgemeiner Grün-Schwellenwert
    (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  # Mindestanzahl an Pixeln, die in einem Blob enthalten sein müssen
fb_merge_margin = 5  # wie nah Blobs pixelweise sein können, bevor sie zusammengeführt werden

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

e_lab_color_thresholds = []  # aktivierte Schwellenwerte
e_lab_color_code = []  # aktivierter Farbcode
e_lab_color_signatures = []  # ursprüngliche aktivierte Schwellenwertindizes
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)

# Kamera-Setup
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)

# LED-Setup
red_led = pyb.LED(1)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)

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

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

if dac:
    dac.write(0)

# Servo-Setup
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))  # Mitte
s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2))  # Mitte

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


# Link-Setup
bus = pyb.I2C(2, pyb.I2C.SLAVE, addr=i2c_address)


def write(data):
    # Die zu übertragenden Daten zuerst vorbereiten, damit es schnell geht.
    out_data = []
    for i in range(0, len(data), 2):
        out_data.append(data[i : i + 2])
    # Interrupts deaktivieren, damit alle Pakete ohne Lücken gesendet werden können.
    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  # Nicht implementiert, da es für uns keine Möglichkeit gibt, bereit zum Empfang der Daten zu sein.


def read_byte():
    return 0  # Nicht implementiert, da es für uns keine Möglichkeit gibt, bereit zum Empfang der Daten zu sein.


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


# FSM-Code
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
        # Ignorieren...

    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


# Hauptschleife
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:  # Farbcodes mit zwei oder mehr Farben oder reguläre
        return (bits_set(blob.code) > 1) or (not color_code(blob.code))
    elif pri_color_code_mode == 2:  # nur Farbcodes mit zwei oder mehr Farben
        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,
            ),
        )
    )

    # Blobs übertragen
    if blobs and (max_blocks > 0) and (max_blocks_per_signature > 0):  # neues Einzelbild
        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)  # alle Daten in einem Paket schreiben...

    else:  # nichts gefunden
        write(struct.pack("<h", 0x0000))

        if dac:
            dac.write(0)

    # Befehle parsen
    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()))