openmv识别红绿蓝物块颜色，并输出物块距离

# 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