#?Untitled?-?By:?lenovo?-?周五?8月?9?2019

import?sensor?image?time
from?pid?import?PID
from?pyb?import?Servo

pan_servo=Servo（1）?#?P7
distance_servo=Servo（2）?#?P8
mid_Xangel?=?-10?#X舵機(jī)給定初始轉(zhuǎn)向
mid_Yangel?=?15?#Y舵機(jī)給定初始轉(zhuǎn)向

Aim_threshold?=?（66?83?17?29?-11?5）?????#（76?87?18?42?-16?4）
red_threshold??=?（67?77?11?32?-20?-6）
pixels_threshold??=?10???#像素的閾值
K=?4840???????????#the?value?should?be?measured
distance_angelvalue?=?[]

#pan_pid?=?PID（p=0.07?i=0?imax=90）?#脫機(jī)運(yùn)行或者禁用圖像傳輸，使用這個PID
#distance_pid?=?PID（p=0.05?i=0?imax=90）?#脫機(jī)運(yùn)行或者禁用圖像傳輸，使用這個PID
pan_pid?=?PID（p=0.1?i=0?imax=90）#在線調(diào)試使用這個PID
distance_pid?=?PID（p=0.1?i=0?imax=90）#在線調(diào)試使用這個PID

sensor.reset（）?#?Initialize?the?camera?sensor.
sensor.set_pixformat（sensor.RGB565）?#?use?RGB565.
sensor.set_framesize（sensor.QVGA）?#?use?QQVGA?for?speed.
sensor.skip_frames（10）?#?Let?new?settings?take?affect.
sensor.set_auto_whitebal（False）?#?turn?this?off.
clock?=?time.clock（）?#?Tracks?FPS.
pan_servo.angle（mid_Xangel）#?擺正水平舵機(jī)
distance_servo.angle（mid_Yangel）?#?擺正豎直舵機(jī)

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（）?#?Track?elapsed?milliseconds?between?sna