import sensor, image, time,pyb
from pyb import Pin, Timer

thresholds = [(24, 71, 2, 63, 5, 127)]

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking

clock = time.clock()
i=0

def beer():
p = Pin('P7') # P7 has TIM4, CH1
tim = Timer(4, freq=1000)
ch = tim.channel(1, Timer.PWM, pin=p)
ch.pulse_width_percent(50)

def timee():
tim=Timer(2,freq=1)
tim.counter()

def tick(timer):
p = Pin('P7') # P7 has TIM4, CH1
tim = Timer(4, freq=1000)
ch = tim.channel(1, Timer.PWM, pin=p)
ch.pulse_width_percent(50)

tim = Timer(2, freq=1) # 使用定时器2创建定时器对象-以1Hz触发

while(True):
clock.tick()
flag = [1,1]
img = sensor.snapshot().lens_corr(1.8)

blob1 = img.find_blobs(thresholds, roi=(123,51,50,70),pixels_threshold=330, area_threshold=500, merge=False)
img.draw_rectangle((123,51,50,70), color=(0,255,0))
blob2 = img.find_blobs(thresholds, roi=(211,51,50,70),pixels_threshold=330, area_threshold=500, merge=False)
img.draw_rectangle((211,51,50,70), color=(0,255,0))


if blob1:
    flag[0]=0
if blob2:
    flag[1]=0




if(flag[0]==0 and flag[1]==0 ):

    print('两个')
    beer()


if(flag[0]==1 and flag[1]==1):
    print('无')

import sensor, image, time
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) #在色块检测模式下关闭自动补光
from pid import PID
from pyb import Servo
clock = time.clock()
from pyb import UART
import ustruct
enable_lens_corr = False
"""    #以下三排根据需求更改
red_threshold_01 = ((35, 100, 41, 77, 24, 59));
green_threshold_01 = ((50, 100, -80, -20, 8, 20));
blue_threshold_01 = ((20, 100, -18, 18, -80, -30));
"""
red_threshold_01 = ((35, 100, 41, 77, 24, 59));
green_threshold_01 = ((50, 100, -80, -20, 8, 17));
blue_threshold_01 = ((46, 95, -34, 19, -45, -11));
thresholds = [(0, 50, -24,-1, -18, 6)]
pan_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
#tilt_pid = PID(p=0.05, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
#pan_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
tilt_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
uart = UART(3,115200)
uart.init(115200, bits=8, parity=None, stop=1)

k=12400  #********************k的值需要实际计算*********   距离*像素宽 单位是厘米
kk=0.326 #直径/像素宽  单位是厘米
def sending_data(flag,cr,yell,k2,k4):
    global uart;
    data = ustruct.pack("<bbhhhhhb",     #b signed char  整数 1
                   0x2C,                 #h short  整数 2
                   0x12,                 #i int 整数  4
                   int(flag),
                   int(cr),
                   int(yell),
                   int(k2),
                   int(k4),
                   0x5B)
    uart.write(data);

def find_max(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_blob

while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8)
    blobs = img.find_blobs([red_threshold_01], pixels_threshold=100, area_threshold=100, merge=True, margin=10);
    blobs1 = img.find_blobs([green_threshold_01], pixels_threshold=100, area_threshold=100, merge=True, margin=10);
    blobs2 = img.find_blobs([blue_threshold_01], pixels_threshold=100, area_threshold=100, merge=True, margin=10);
    cx=0;cy=0;cr=0;flag=0;yellow=0;k2=0;k4=0;
    if blobs:
        yellow = 1; #R
    elif blobs1:
        yellow = 2; #G
    else:
        yellow = 3; #B
    print("yellow", yellow)
    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):
        if yellow==1:
            img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))
        elif yellow==2:
            img.draw_circle(c.x(), c.y(), c.r(), color = (0, 255, 0))
        else :
            img.draw_circle(c.x(), c.y(), c.r(), color = (0, 0, 255))
        cx = c.x();
        cy = c.y();
        cr = c.r();
        #k1=k/2/cr     #圆形实际长度=k/圆形直径的像素点长度
        k2=kk*2*cr  #距离=kk*圆形直径的像素点长度
        flag = 1;
        #print("距离为",k1,"厘米")
        print("直径为",k2,"厘米")
        time.sleep(100)
        print("圆形",c)
        break
    for r in img.find_rects(threshold = 15000):
        if yellow==1:
            img.draw_rectangle(r.rect(), color = (255, 0, 0))
            #for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (255, 0, 0))
        elif yellow==2:
            img.draw_rectangle(r.rect(), color = (0, 255, 0))
           # for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
        else:
            img.draw_rectangle(r.rect(), color = (0, 0, 255))
           # for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 0, 255))
        cx = int(r.x()+(r.w()/2))
        cy = int(r.y()+(r.h()/2))
        flag =2
        print(r)
        print("矩形： x:",r.x()+(r.w()/2),"y:",int(r.y()+(r.h()/2)))
        time.sleep(100)
        #k3=k/cx     #矩形实际长度=k/矩形像素点长度
        k4=kk*cx   #距离=kk*矩形直径的像素点长度
        #print("距离为",k3,"厘米")
        print("长度为",k4,"厘米")
        break
    for s in img.find_line_segments(merge_distance = 10, max_theta_diff = 10):
        if yellow==1:
            img.draw_line(s.line(), color = (255, 0, 0))
        elif yellow==2:
            img.draw_line(s.line(), color = (0, 255, 0))
        else:
            img.draw_line(s.line(), color = (0, 0, 255))
        if flag==1:
            break;
        elif flag==2:
            break;
        print(s)
        print("三角形")
        num_segment=1
        flag =3
        time.sleep(1000)
        break
    if flag == 0:
            print(flag)
    sending_data(flag,cr,yellow,k2,k4);