import sensor, image, time, math,json
from pyb import LED
from pyb import UART
import ustruct
thresholds = [(11, 56, 16, 59, -8, 44),#red
(22, 50, -3, 14, 11, 36),
(13, 43, -4, 29, -89, -21)]#blue
objthresholds = [(34, 42, 26, 54, 5, 25), # generic_red_thresholds
(56, 67, -39, -17, 12, 37), # generic_green_thresholds
(29, 45, 0, 14, -32, -15)] # generic_blue_thresholds
graythreshold=[(100,255)]
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)
clock = time.clock()
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)
uart1 = UART(1, 115200, timeout_char=1000)
uart1.init(115200, bits=8, parity=None, stop=1,timeout_char=1000)
A=0
red_cx = 0
green_cx = 0
blue_cx = 0
a = 0
led = LED(1)
led1 = LED(2)
led2 = LED(3)
led3 = LED(4)
def sending_data(cx,cy):
global uart;
#frame=[0x2C,18,cx%0xff,int(cx/0xff),cy%0xff,int(cy/0xff),0x5B];
#data = bytearray(frame)
data = ustruct.pack("<bbhhb",
0x2C,
0x12,
int(cx),
int(cy),
0x5B)
uart.write(data)
# 比较两个色块大小的函数
def compareBlob(blob1, blob2):
# 这里我们选择了pixels作为指标比对二者的代码
# 你也可以换用其它指标 例如 blob.area()
tmp = blob1.pixels() - blob2.pixels()
if tmp == 0:
return 0;
elif tmp > 0:
return 1;
else:
return -1;
while (True):
led.on()
if(uart1.any()):
a = uart1.readchar()
if(a==65): #A
A=1
led1.on()
if(a==66): #B
A=0
if(a==67): #C
A=2
led.on()
if(a==68): #D
A=3
led1.on()
if(a==69): #E
A=4
led2.on()
print(a)
print('A = %d'%A)
if(A==1):
led1.on()
led.off()
clock.tick()
img = sensor.snapshot()
for blob in img.find_blobs([thresholds[0]], pixels_threshold=500, area_threshold=500, merge=True):
img.draw_rectangle(blob[0:4])
img.draw_cross(blob.cx(), blob.cy())
red_cx = blob.cx()
for blob in img.find_blobs([thresholds[1]], pixels_threshold=500, area_threshold=500, merge=True):
img.draw_rectangle(blob[0:4])
img.draw_cross(blob.cx(), blob.cy())
green_cx = blob.cx()
for blob in img.find_blobs([thresholds[2]],pixels_threshold=500, area_threshold=500, merge=True):
img.draw_rectangle(blob[0:4])
img.draw_cross(blob.cx(), blob.cy())
blue_cx = blob.cx()
if red_cx > green_cx and green_cx > blue_cx :
output_str='r123'
print(output_str)
uart.write(output_str)
time.sleep(4)
if red_cx > blue_cx and blue_cx > green_cx :
output_str='r132'
print(output_str)
uart.write(output_str)
time.sleep(4)
if green_cx > blue_cx and blue_cx > red_cx :
output_str='r231'
print(output_str)
uart.write(output_str)
time.sleep(4)
if green_cx > red_cx and red_cx > blue_cx :
output_str='r213'
print(output_str)
uart.write(output_str)
time.sleep(4)
if blue_cx > green_cx and green_cx > red_cx :
output_str='r321'
print(output_str)
uart.write(output_str)
time.sleep(4)
if blue_cx > red_cx and red_cx > green_cx :
output_str='r312'
print(output_str)
uart.write(output_str)
time.sleep(4)
if(A==2 ):
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
img.binary([objthresholds[0]])
img.dilate(4)
blobs=img.find_blobs(graythreshold, pixels_threshold=2025, area_threshold=1600, merge=True)
if len(blobs):
bigBlob = blobs[0]
for blob in blobs:
if compareBlob(bigBlob, blob) == -1:
bigBlob = blob
img.draw_rectangle(bigBlob.rect())
print(bigBlob.cx(),bigBlob.cy())
sending_data(bigBlob.cx(),bigBlob.cy())
time.sleep(4)
if(A==3 ):
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
img.binary([objthresholds[1]])
img.dilate(4)
blobs=img.find_blobs(graythreshold, pixels_threshold=2025, area_threshold=1600, merge=True)
if len(blobs):
bigBlob = blobs[0]
for blob in blobs:
if compareBlob(bigBlob, blob) == -1:
bigBlob = blob
img.draw_rectangle(bigBlob.rect())
print(bigBlob.cx(), bigBlob.cy())
sending_data(bigBlob.cx(),bigBlob.cy())
time.sleep(4)
if(A==4 ):
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
img.binary([objthresholds[2]])
img.dilate(4)
blobs=img.find_blobs(graythreshold, pixels_threshold=900, area_threshold=900, merge=True)
if len(blobs):
bigBlob = blobs[0]
for blob in blobs:
if compareBlob(bigBlob, blob) == -1:
bigBlob = blob
img.draw_rectangle(bigBlob.rect())
print(bigBlob.cx(), bigBlob.cy())
sending_data(bigBlob.cx(),bigBlob.cy())
time.sleep(4)