mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
This was necessary due to the increase in the frame rate. The previous method did not correlate to time. All scripts updated.
338 lines
11 KiB
Python
338 lines
11 KiB
Python
# Pixy SPI Emulation Script
|
|
#
|
|
# This script allows your OpenMV Cam to emulate the Pixy (CMUcam5) in SPI mode.
|
|
# Note that you need to setup the lab color thresholds below for your application.
|
|
#
|
|
# P0 = MOSI
|
|
# P1 = MISO
|
|
# P2 = SCLK
|
|
# P3 = SS
|
|
#
|
|
# P7 = Servo 1
|
|
# P8 = Servo 2
|
|
|
|
# Note: Please make sure your Arduino, etc. is running its SPI code before the
|
|
# OpenMV Cam starts running. Additionally, sometimes the OpenMV Cam is
|
|
# unable to sync up with the Arduino SPI data stream. Just restart your
|
|
# OpenMV Cam when this happens. The OpenMV Cam should sync up within
|
|
# 1-9 tries. Use the serial or I2C version of this script otherwise.
|
|
|
|
# Pixy Parameters ############################################################
|
|
|
|
color_code_mode = 1 # 0 == Disabled, 1 == Enabled, 2 == Color Codes Only, 3 == Mixed
|
|
|
|
max_blocks = 1000
|
|
max_blocks_per_signature = 1000
|
|
min_block_area = 20
|
|
|
|
# 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 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(time = 2000)
|
|
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("<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
|
|
# Ignore...
|
|
|
|
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
|
|
|
|
# Main Loop
|
|
|
|
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): # color codes with two or more colors or regular
|
|
return (bits_set(blob.code()) > 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("<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_rectangle(blob.rect())
|
|
img.draw_cross(blob.cx(), blob.cy())
|
|
|
|
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) # write all data in one packet...
|
|
|
|
else: # nothing found
|
|
write(struct.pack("<h", 0x0000))
|
|
|
|
if dac:
|
|
dac.write(0)
|
|
|
|
# Parse Commands #
|
|
|
|
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()))
|