import sensor
import time
import json
import ustruct
import sensor, image,math,pyb,ujson
from pyb import UART
from pyb import LED
sensor.reset() # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA) # Set frame size to QVGA (320x240)
sensor.skip_frames(time=2000) # Wait for settings take effect.
clock = time.clock()
red1_404 = (31, 58, 48, 74, 20, 50)
red2_402 =(34, 64, 23, 65, 5, 39)
red3_402 =(32, 74, 23, 68, 7, 39)
red4_402 = (31, 58, 48, 74, 20, 50)
red5_402 = (32, 74, 23, 68, 7, 39)
red6_404 = (31, 58, 48, 74, 20, 50)
#red1_404,red2_402,red3_402,red4_402,red5_402,red6_404
green1 =(63, 88, -44, -9, -128, 127) # green_thresholds
green2 = (20, 61, -128, -11, -128, 127)
green3 = (20, 68, -128, -11, -128, 127)
green1_402 = (53, 100, -128, -11, 0, 36)
green2_402 =(53, 101, -128, -11, 0, 36)
green3_402 =(53, 100, -127, -11, 0, 36)
green4_402 = (53, 100, -128, -12, 0, 36)
green5_402 = (52, 100, -128, -11, 1, 36)
green6_404 =(39, 92, -128, 5, -17, 34)
#green1_402,green2_402,green3_402,green4_402,green5_402,green6_404
blue1=(0, 27, 5, 127, -128, -19)# blue_thresholds
blue2 = (0, 28, 0, 127, -128, -16)
blue3 = (0, 28, 4, 127, -128, -19)
blue1_402 = (25, 65, 12, 44, -74, -41)
blue2_402 = (25, 65, 12, 44, -74, -41)
blue3_402 = (25, 65, 12, 44, -74, -41)
blue4_402 = (25, 65, 12, 44, -74, -41)
blue5_402 = (25, 65, 12, 44, -74, -41)
blue6_404 = (25, 65, 12, 44, -74, -41)
#blue1_402,blue2_402,blue3_402,blue4_402,blue5_402,blue6_404
#初始化变量
uart = UART(3,115200)
uart.init(115200, bits=8, parity=None, stop=1)
pi=3.1415926
data = 0x00
tx_data = 0
gray_ground1 =(3, 44, -10, 8, -15, 5)
gray_ground2 =(0, 67, -15, 25, -35, -9)
rx_buff=[]
state = 0
def Receive_Prepare():
global data
global state
if(uart.any()>0):
data=uart.readchar()
if state==0:
if data == 0x0d:#帧头
state = 1
else:
state = 0
rx_buff.clear()
elif state==1:
rx_buff.append(data)
state = 2
elif state == 2:
if data == 0x5b:
tx_data = int(rx_buff[0])
state = 0
else:
state = 0
rx_buff.clear()
def sending_data(cx,cy,c,R):
global uart
data = bytearray([0xFF,0xFF,cx,cy,c,R,R,0xFE])
uart.write(data) #必须要传入一个字节数组
print("send data")
def find_max(blobs): #定义寻找色块面积最大的函数
max_size=0
for blob in blobs:
if blob.pixels() > max_size:
max_blob=blob
max_size = blob.pixels()
return max_blob
def receive_color_and_send_message(color_threshold1,color_threshold2,colorthreshold3,colorthreshold4,colorthreshold5,colorthreshold6):
#检测物块位置
global tx_data
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 1500)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
sensor.set_vflip(False)
sensor.set_hmirror(False)
while(tx_data == 1):
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
blobs = img.find_blobs([color_threshold1,color_threshold2,colorthreshold3,colorthreshold4,colorthreshold5,colorthreshold6],pixels_threshold=13000)
if blobs:
max_b = find_max(blobs)#找到最大的颜色矩形
#img.draw_rectangle(max_b.rect())
#img.draw_cross(max_b.cx(), max_b.cy())
x = max_b.density()#显示颜色的可靠程度
if(x<0.3):
c=1
elif(x>0.3 or x == 0.3):
c=2
cx = max_b.cx()
cy = max_b.cy()
pyb.delay(10)#拉长数据发送时间，让芯片来得及处理数据
sending_data(cx,cy,c,1)
Receive_Prepare()
#print(cx)
else:
sending_data(0,0,0,0)
Receive_Prepare()
def jiaozhun():
global tx_data
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
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
sensor.set_vflip(False)
sensor.set_hmirror(False)
while(tx_data == 4):
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
for circle in img.find_circles(threshold = 4000,r_min = 30,r_max=40, x_margin = 5, y_margin = 5, r_margin =1, r_step = 2):
img.draw_circle(circle.x(), circle.y(), circle.r(),color = (255,255,255) )
area = (circle.x()-circle.r(), circle.y()-circle.r(), 2*circle.r(), 2*circle.r())
cx=circle.x()
cy=circle.y()
cr = circle.r()
c = 1
R = 2
print("cx is ")
print(cx)
print("cy is ")
print(cy)
print(cr)
sending_data(cx,cy,c,R)
receive_data()
def whether_pingxing():
global tx_data
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
sensor.set_vflip(False)
sensor.set_hmirror(False)
while(tx_data == 5):
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
blobs = img.find_blobs([gray_ground1,gray_ground2],pixels_threshold=11000)
if blobs:
max_b = find_max(blobs)
img.draw_rectangle(max_b.rect(), color=(0,0,0))
#print(max_b.rotation()*180/pi)
Rotation = int(max_b.rotation() *360/pi)
if(Rotation >180):
Rotation = abs(Rotation - 360)
R = 1
else:
R=0
Distance = max_b.h()
sending_data(Rotation,Distance,1,R)
print("rotation we send is ")
print(Rotation)
#print("distance we send is")
#print(Distance)
receive_data()
else:
sending_data(0,0,0,0)
while True:
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
Receive_Prepare()
if (tx_data == 1):#red
print("here 1")
red()
receive_color_and_send_message(red1_404,red2_402,red3_402,red4_402,red5_402,red6_404)
elif(tx_data == 2):
print("here 2")
green()
receive_color_and_send_message(green1_402,green2_402,green3_402,green4_402,green5_402,green6_404)
elif(tx_data == 3):
print("here 3")
blue()
receive_color_and_send_message(blue1_402,blue2_402,blue3_402,blue4_402,blue5_402,blue6_404)
elif(tx_data == 4):#检测圆心
print("here 4")
green()
jiaozhun()
elif(tx_data == 5):
print("here 5")
red()
whether_pingxing()
else:
print("bad data")
sending_data(0,0,0,0)