import sensor, image, time ,math ,struct
from pyb import UART
from struct import pack, unpack
import json
red_threshold =(45, 70, 47, 127, -128, 17)#(8, 95, 29, 79, 12, 68)(9, 90, 52, 79, 12, 68)#(7, 90, 12, 70, -6, 41)#(15, 100, 40, 100, 40, 80)#(100, 27, 28, 111, 94, -59)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time=3000)
sensor.set_auto_whitebal(False)
clock = time.clock()
uart = UART(1, 115200)
#def find_max(blobs):
#max_size=1
#if blobs:
#max_blob = 0
#for blob in blobs:
#blob_size = blob.w()*blob.h()
#if ( (blob_size > max_size) & (blob_size > 100) & (blob.density()>0.8*math.pi/4) & (blob.density()<1.2*math.pi/4) ) :
#if ( math.fabs( blob.w() / blob.h() - 1 ) < 0.5 ) :
#max_blob=blob
#max_size = blob.w()*blob.h()
#return max_blob
#def find_red(blobs):
#max_size = 0
#for blob in blobs:
#if blob[2]*blob[3] > max_size:
#max_blob=blob
#max_size = blob[2]*blob[3]
#return max_size
def line_filter_copy(src, dst):
for i in range(0, len(dst), 1):
dst[i] = src[i<<1]
def line_filter_bw(src, dst):
for i in range(0, len(dst), 1):
if (src[i<<1] > 200 and src[i<<1] < 255):
dst[i] = 0xFF
else:
dst[i] = 0x00
k = 0
max_number = 0
a = 0
b = 0
min_yellow = 0
tmp_data = 0
while(True):
print(1)
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,
r_min = 2, r_max = 100, r_step = 2):
area = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
print(2)
#area为识别到的圆的区域，即圆的外接矩形框
statistics = img.get_statistics(roi=area)#像素颜色统计 绿色 (0, 73, -108, 14, -128, 127) 白色(74, 100, -21, 64, -30, 34) hong (21, 71, 34, 127, -128, 127)
#(0,100,0,120,0,120)是红色的阈值，所以当区域内的众数（也就是最多的颜色），范围在这个阈值内，就说明是红色的圆。 粉红(70, 100, 10, 48, -37, 6)(45, 70, 47, 127, -128, 17)
#l_mode()，a_mode()，b_mode()是L通道，A通道，B通道的众数。
if 0<statistics.l_mode()<70 and 0<statistics.a_mode()<127 and 0<statistics.b_mode()<17:#if the circle is red
img.draw_circle(c.x(), c.y(), c.r(), color = (0, 255, 0))#识别到的红色圆形用红色的圆框出来
print(3)
clock.tick()
lines = 0
img = sensor.snapshot(line_filter = line_filter_copy)
#red_blobs = img.find_blobs([red_threshold])
#max_blob=find_max(red_blobs)
last_x = 1000
last_y = 1000
if area:
bx_x=circle.x()-160
by_y=120-circle.r()
x=math.fabs(bx_x)
y=math.fabs(by_y)
if 0 <= x < 10 :
bx_value = 1
elif 10 <= x < 100 :
bx_value = 2
else :
bx_value = 3
if 0 <= y < 10 :
by_value = 1
elif 10 <= y < 100 :
by_value = 2
else :
by_value = 3
sumA = 0
sumB = 0
data = bytearray([0x41,0x43])
uart.write(data)
data = bytearray([0x02,8])
for b in data:
sumB = sumB + b
sumA = sumA + sumB
uart.write(data)
float_value = bx_x
float_bytes = pack('f', float_value)
for b in float_bytes:
sumB = sumB + b
sumA = sumA + sumB
uart.write(float_bytes)
float_value = by_y
float_bytes = pack('f', float_value)
for b in float_bytes:
sumB = sumB + b
sumA = sumA + sumB
uart.write(float_bytes)
data = bytearray([sumB, sumA])
uart.write(data)
print("found: x=",bx_x," y=",by_y)
#b_output_str="x%d%d,y%d%d" % (bx_value,bx_x,by_value,by_y)
#print('you send black:',b_output_str)
#uart.write(b_output_str+'\r\n')
else:
sumA = 0
sumB = 0
data = bytearray([0x41,0x43])
uart.write(data)
data = bytearray([0x01,0])
for b in data:
sumB = sumB + b
sumA = sumA + sumB
uart.write(data)
data = bytearray([sumB, sumA])
uart.write(data)
print(sumA," ",sumB)
#print('not found!')
#b_output_str = "x10,y10"
#print('you send black:',b_output_str)
#b_output_str = "x3750,y3750"
#uart.write(b_output_str+'\r\n')
#pass
else:
img.draw_rectangle(area, color = (255, 255, 255))
print(4)