From 06f41880a787ead816bb4423e6375f19a56d21b8 Mon Sep 17 00:00:00 2001 From: "Kwabena W. Agyeman" Date: Fri, 10 Mar 2017 19:24:33 -0500 Subject: [PATCH] Add pixy emulation scripts --- .../apriltags_pixy_i2c_emulation.py | 245 +++++++++++++ .../apriltags_pixy_spi_emulation.py | 253 +++++++++++++ .../apriltags_pixy_uart_emulation.py | 227 ++++++++++++ .../17-Pixy-Emulation/pixy_i2c_emulation.py | 329 +++++++++++++++++ .../17-Pixy-Emulation/pixy_spi_emulation.py | 337 ++++++++++++++++++ .../17-Pixy-Emulation/pixy_uart_emulation.py | 311 ++++++++++++++++ 6 files changed, 1702 insertions(+) create mode 100644 usr/examples/17-Pixy-Emulation/apriltags_pixy_i2c_emulation.py create mode 100644 usr/examples/17-Pixy-Emulation/apriltags_pixy_spi_emulation.py create mode 100644 usr/examples/17-Pixy-Emulation/apriltags_pixy_uart_emulation.py create mode 100644 usr/examples/17-Pixy-Emulation/pixy_i2c_emulation.py create mode 100644 usr/examples/17-Pixy-Emulation/pixy_spi_emulation.py create mode 100644 usr/examples/17-Pixy-Emulation/pixy_uart_emulation.py diff --git a/usr/examples/17-Pixy-Emulation/apriltags_pixy_i2c_emulation.py b/usr/examples/17-Pixy-Emulation/apriltags_pixy_i2c_emulation.py new file mode 100644 index 000000000..5c6c1be02 --- /dev/null +++ b/usr/examples/17-Pixy-Emulation/apriltags_pixy_i2c_emulation.py @@ -0,0 +1,245 @@ +# AprilTags Pixy I2C Emulation Script +# +# This script allows your OpenMV Cam to transmit AprilTag detection data like +# a Pixy (CMUcam5) tracking colors in I2C mode. This script allows you to +# easily replace a Pixy (CMUcam5) color tracking sensor with an OpenMV Cam +# AprilTag tracking sensor. Note that this only runs on the OpenMV Cam M7. +# +# P4 = SCL +# P5 = SDA +# +# P7 = Servo 1 +# P8 = Servo 2 + +# Note: The tag family is TAG36H11. Additionally, in order to for the +# signature value of a tag detection to be compatible with pixy +# interface libraries all tag ids have 8 added to them in order +# to move them in the color code signature range. Finally, tags +# are all reported as color code blocks... + +# Pixy Parameters ############################################################ + +max_blocks = 1000 +max_blocks_per_id = 1000 + +i2c_address = 0x54 + +# Pan Servo +s0_lower_limit = 1000 # Servo pulse width lower limit in microseconds. +s0_upper_limit = 2000 # Servo pulse width upper limit in microseconds. + +# Tilt Servo +s1_lower_limit = 1000 # Servo pulse width lower limit in microseconds. +s1_upper_limit = 2000 # Servo pulse width upper limit in microseconds. + +analog_out_enable = False # P6 -> Analog Out (0v - 3.3v). +analog_out_mode = 0 # 0 == x position of largest tag - 1 == y position of largest tag + +############################################################################## + +import image, math, pyb, sensor, struct, time + +# Camera Setup + +sensor.reset() +sensor.set_pixformat(sensor.GRAYSCALE) +sensor.set_framesize(sensor.QQVGA) +sensor.skip_frames(60) + +# 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)) # center +s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2)) # center + +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): + # Prepare the data to transmit first so we can do it quickly. + out_data = [] + for i in range(0, len(data), 2): + out_data.append(data[i:i+2]) + # Disable interrupts so we can send all packets without gaps. + 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 # Not implemented as there is no way for the us to be ready to receive the data. + +def read_byte(): + return 0 # Not implemented as there is no way for the us to be ready to receive the data. + +# Helper Stuff + +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 to_object_block_format(tag): + angle = int((tag.rotation() * 180) // math.pi) + temp = struct.pack(" 0) and (max_blocks_per_id > 0): # new frame + dat_buf = struct.pack(" Analog Out (0v - 3.3v). +analog_out_mode = 0 # 0 == x position of largest tag - 1 == y position of largest tag + +############################################################################## + +import image, math, pyb, sensor, struct, time + +# Camera Setup + +sensor.reset() +sensor.set_pixformat(sensor.GRAYSCALE) +sensor.set_framesize(sensor.QQVGA) +sensor.skip_frames(60) + +# 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)) # center +s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2)) # center + +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.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 # Not implemented as there is no way for the us to be ready to receive the data. + +def read_byte(): + return 0 # Not implemented as there is no way for the us to be ready to receive the data. + +# Helper Stuff + +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 to_object_block_format(tag): + angle = int((tag.rotation() * 180) // math.pi) + temp = struct.pack(" 0) and (max_blocks_per_id > 0): # new frame + dat_buf = struct.pack(" Analog Out (0v - 3.3v). +analog_out_mode = 0 # 0 == x position of largest tag - 1 == y position of largest tag + +############################################################################## + +import image, math, pyb, sensor, struct, time + +# Camera Setup + +sensor.reset() +sensor.set_pixformat(sensor.GRAYSCALE) +sensor.set_framesize(sensor.QQVGA) +sensor.skip_frames(60) + +# 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)) # center +s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2)) # center + +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 + +uart = pyb.UART(3, uart_baudrate, timeout_char = 1000) + +def write(data): + uart.write(data) + +def available(): + return 0 # Not implemented as there is no way for the us to be ready to receive the data. + +def read_byte(): + return 0 # Not implemented as there is no way for the us to be ready to receive the data. + +# Helper Stuff + +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 to_object_block_format(tag): + angle = int((tag.rotation() * 180) // math.pi) + temp = struct.pack(" 0) and (max_blocks_per_id > 0): # new frame + dat_buf = struct.pack(" Analog Out (0v - 3.3v). +analog_out_mode = 0 # 0 == x position of largest blob - 1 == y position of largest blob + +# 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 - Is Color Code Threshold? (True/False). +# Parameter 7 - Enable Threshold? (True/False). +lab_color_thresholds = [(0, 100, 40, 127, -128, 127, True, True), # Generic Red Threshold + (0, 100, -128, -10, -128, 127, True, True), # Generic Green Threshold + (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 # minimum number of pixels that must be in a blob +fb_merge_margin = 5 # how close pixel wise blobs can be before merging + +############################################################################## + +e_lab_color_thresholds = [] # enabled thresholds +e_lab_color_code = [] # enabled color code +e_lab_color_signatures = [] # original enabled threshold indexes +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) + +import image, math, pyb, sensor, struct, time + +# Camera Setup + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.QVGA) +sensor.skip_frames(60) +sensor.set_auto_gain(False) +sensor.set_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)) # center +s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2)) # center + +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): + # Prepare the data to transmit first so we can do it quickly. + out_data = [] + for i in range(0, len(data), 2): + out_data.append(data[i:i+2]) + # Disable interrupts so we can send all packets without gaps. + 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 # Not implemented as there is no way for the us to be ready to receive the data. + +def read_byte(): + return 0 # Not implemented as there is no way for the us to be ready to receive the data. + +# Helper Stuff + +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(" 1) or (not color_code(blob.code())) + elif(pri_color_code_mode == 2): # only color codes with two or more colors + return (bits_set(blob.code()) > 1) + elif(pri_color_code_mode == 3): + return True + +clock = time.clock() +while(True): + clock.tick() + img = sensor.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))) + + # Transmit Blobs # + + if blobs and (max_blocks > 0) and (max_blocks_per_signature > 0): # new frame + dat_buf = struct.pack(" Analog Out (0v - 3.3v). +analog_out_mode = 0 # 0 == x position of largest blob - 1 == y position of largest blob + +# 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 - Is Color Code Threshold? (True/False). +# Parameter 7 - Enable Threshold? (True/False). +lab_color_thresholds = [(0, 100, 40, 127, -128, 127, True, True), # Generic Red Threshold + (0, 100, -128, -10, -128, 127, True, True), # Generic Green Threshold + (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 # minimum number of pixels that must be in a blob +fb_merge_margin = 5 # how close pixel wise blobs can be before merging + +############################################################################## + +e_lab_color_thresholds = [] # enabled thresholds +e_lab_color_code = [] # enabled color code +e_lab_color_signatures = [] # original enabled threshold indexes +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) + +import image, math, pyb, sensor, struct, time + +# Camera Setup + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.QVGA) +sensor.skip_frames(60) +sensor.set_auto_gain(False) +sensor.set_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)) # center +s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2)) # center + +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.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 # Not implemented as there is no way for the us to be ready to receive the data. + +def read_byte(): + return 0 # Not implemented as there is no way for the us to be ready to receive the data. + +# Helper Stuff + +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(" 1) or (not color_code(blob.code())) + elif(pri_color_code_mode == 2): # only color codes with two or more colors + return (bits_set(blob.code()) > 1) + elif(pri_color_code_mode == 3): + return True + +clock = time.clock() +while(True): + clock.tick() + img = sensor.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))) + + # Transmit Blobs # + + if blobs and (max_blocks > 0) and (max_blocks_per_signature > 0): # new frame + dat_buf = struct.pack(" Analog Out (0v - 3.3v). +analog_out_mode = 0 # 0 == x position of largest blob - 1 == y position of largest blob + +# 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 - Is Color Code Threshold? (True/False). +# Parameter 7 - Enable Threshold? (True/False). +lab_color_thresholds = [(0, 100, 40, 127, -128, 127, True, True), # Generic Red Threshold + (0, 100, -128, -10, -128, 127, True, True), # Generic Green Threshold + (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 # minimum number of pixels that must be in a blob +fb_merge_margin = 5 # how close pixel wise blobs can be before merging + +############################################################################## + +e_lab_color_thresholds = [] # enabled thresholds +e_lab_color_code = [] # enabled color code +e_lab_color_signatures = [] # original enabled threshold indexes +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) + +import image, math, pyb, sensor, struct, time + +# Camera Setup + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.QVGA) +sensor.skip_frames(60) +sensor.set_auto_gain(False) +sensor.set_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)) # center +s1_tilt.pulse_width(int((max_s1_limit - min_s1_limit) // 2)) # center + +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 + +uart = pyb.UART(3, uart_baudrate, timeout_char = 1000) + +def write(data): + uart.write(data) + +def available(): + return uart.any() + +def read_byte(): + return uart.readchar() + +# Helper Stuff + +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(" 1) or (not color_code(blob.code())) + elif(pri_color_code_mode == 2): # only color codes with two or more colors + return (bits_set(blob.code()) > 1) + elif(pri_color_code_mode == 3): + return True + +clock = time.clock() +while(True): + clock.tick() + img = sensor.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))) + + # Transmit Blobs # + + if blobs and (max_blocks > 0) and (max_blocks_per_signature > 0): # new frame + dat_buf = struct.pack("