#?Untitled?-?By:?xiong?-?周五?8月?3?2018

import?sensor?image?timepyb
from?pyb?import?UART
import?json
uart?=?UART（3115200）
uart.init（115200?bits=8?parity=None?stop=1）
led?=?pyb.LED（3）
usb?=?pyb.USB_VCP（）
led.on（）
WINDOW_CENTER_X?=?80
WINDOW_CENTER_Y?=?60
#?小球的顏色閾值，根據顏色追蹤小球
target_color_threshold???=?（74?51?13?93?3?81）
#?下面的閾值?可能需要大家手動調節
thresholds?=?[（44?73?94?19?7?53）?#?一般情況下的紅色閾值
??????????????（22?80?-80?-25?-1?76）?#?一般情況下的綠色閾值（16?40?-58?-18?8?50）
??????????????（84?27?9?-10?-80?-28）]?#?一般情況下的藍色閾值（26?71?-27?1?-59?-20）（7?32?-31?26?-82?-12）
??????????????#（2?20?-8?8?-28?15）?#?一般情況下的黑色閾值
??????????????#（67?80?-12?61?-2?19）?#?一般情況下的白色閾值
GRAYSCALE_THRESHOLD?=?[（70?100?-52?59?-44?55）
????????????????????????（37?9?8?-62?-29?20）]#一般情況下的黑色閾值

sensor.reset（）?#?Initialize?the?camera?sensor.
sensor.set_pixformat（sensor.RGB565）?#?use?RGB565.
sensor.set_framesize（sensor.QQVGA）?#?use?QQVGA?for?speed.
sensor.skip_frames（10）?#?Let?new?settings?take?affect.
#sensor.set_auto_whitebal（False）?#?turn?this?off.
#sensor.set_auto_gain（False）?#?must?be?turned?off?for?color?tracking

clock?=?time.clock（）?#?Tracks?FPS.

K=5000?#?K是我們計算出來的常數

color_code?=?0
cx_code?=?0
distance_code?=?0
cx_bandw_code?=?0
def?calc_radius（blob）:
????#?計算圖像中色塊的半徑??比較粗暴
????return?（blob.w（）?+?blob.h（））?/?2

def?get_distance（colorblobs）:
????#?尋找目標顏色的小球
????????global?distance_codecx_code
????????img?=?sensor.snapshot（）
????????if?blobs?!=?1:
????????????#?Draw?a?rect?around?the?blob.
????????????#blob?=?blobs[0]
????????????#img.draw_rectangle（blob.rect（））
????????????#img.draw_cross（blob.cx（）?blob.cy（））
????????????#?圖像中小球的半徑
????????????img_ball_r=?calc_radius（blobs）
????????????#?小球離鏡頭的距離?根據我們的公式計算
????????????ball_distance?=?K?/?img_ball_r
????????????#print（“小球距離:?%d“%ball_distance）
????????????distance_code?=?int（ball_distance）
????????????cx_code?=?blobs.cy（）

def?get_color_code（blob_color）:
????if?blob_color?==?1:
????????return?0;
????elif?blob_color?==?2:
????????return?1;
????elif?blob_color?==?4:
????????return?2;

#?繪制實心圓?-?只是為了好看
def?drawFullFillCircle（img?x?y?r?color）:
????#?一圈一圈填實
????for?ri?in?range（1?r?+?1）:
????????img.draw_circle（x?y?ri?color?=?color）

#?標記當前識別的顏色
def?markCurColor（img?cName?cRgb）:
????img.draw_string（0?0?cName?color?=?cRgb）
????drawFullFillCircle（img?40?40?20?cRgb）

#?如果是紅色，執行此函數
def?doWithRed（img）:
????global?color_code
????color_code?=?1
????markCurColor（img?“RED“?（255?0?0））

#?如果是綠色，執行此函數
def?doWithGreen（img）:
????global?color_code
????markCurColor（img?“GREEN“?（0?255?0））
????color_code?=?2

#?如果是綠色，執行此函數
def?doWithBlue（img）:
????global?color_code
????markCurColor（img?“BLUE“?（0?0?255））
????color_code?=?4

#?如果是白色，執行此函數
def?doWithWhite（img）:
????global?color_code
????markCurColor（img?“WHITE“?（255?255?255））
????color_code?=?24

#?如果是黑色，執行此函數
def?doWithBlack