# 多顏色跟蹤示例
#
# 這個例子顯示了使用OpenMV的多色跟蹤。

import sensor, image, time

# 顏色跟蹤閾值(L Min, L Max, A Min, A Max, B Min, B Max)
# 下面的閾值跟蹤一般紅色/綠色的東西。你不妨調整他們...
thresholds = [(47, 68, 55, 103, 25, 63), # red_thresholds
              (60, 75, -80, -40, 30, 50), # green_thresholds
              (29, 49, -5, 25, -63,
 
 -35)] # blue_thresholds

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(True
 
)
sensor.set_hmirror(True)
clock = time.clock()

# 只有比“pixel_threshold”多的畫素和多於“area_threshold”的區域才被
# 下面的“find_blobs”返回。 如果更改相機解析度，
# 請更改“pixels_threshold”和“area_threshold”。 “merge = True”合併影象中所有重疊的色塊。

while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8) 
    for blob in img.find_blobs(
 
thresholds, pixels_threshold=200, area_threshold=200):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
    print(clock.fps())

測距

import sensor, image, time

yellow_threshold   = ( 56,   83,    5,   57,   63,   80)

sensor.reset()
sensor.set_pixformat(sensor.RGB565) 
sensor.set_framesize(sensor.QQVGA) 
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
sensor.set_vflip(True)
sensor.set_hmirror(True)
clock = time.clock() 

K=5000

while(True):
    clock.tick() 
    img = sensor.snapshot().lens_corr(1.8) 

    blobs = img.find_blobs([yellow_threshold])
    if len(blobs) == 1:     
        b = blobs[0]
        img.draw_rectangle(b[0:4]) # rect
        img.draw_cross(b[5], b[6]) # cx, cy
        Lm = (b[2]+b[3])/2
        length = K/Lm
        print(length)

11點30分，2018年12月21日

import sensor, image
import time, math
from servo import Servos
from machine import I2C, Pin
i2c = I2C(sda=Pin('P5'), scl=Pin('P4'))
servo = Servos(i2c, address=0x40, freq=50, min_us=650, max_us=2800, degrees=180)

red_threshold     =   (47, 68, 55, 103, 25, 63)
green_threshold   =   (45, 70, -60, -40, 20, 60)
blue_threshold    =   (29, 49, -5, 25, -63, -35)
global ball_threshold
ball_threshold = green_threshold

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
sensor.set_vflip(True)
sensor.set_hmirror(True)
clock = time.clock()

global count
count=0

global ballcode
ballcode = [0,0,0]

global k#左轉右轉找球
k=1

hand_speed=13
servo_reset=     [40, 110,  80, 30, 10,  140, 0]#出發姿勢
servo_open =     [40, 110,  20,  80, 10,   100, 0]#展開姿勢
servo_camera =     [40, 110,  3,  50, 20,   65, 0]#展開姿勢
servo_up1 =     [40, 110,  10,  100, 10,   100, 0]#展開姿勢
servo_up2 =     [115, 40,  10,  100, 10,   100, 0]#展開姿勢
servo_catchready=[40, 110, 30,  100, 60,   56, 12]#抓取姿勢
servo_catch=     [105,50,  30,  78, 60,   60, 90]#抓取姿勢
servo_set  =     [105, 50,  20,  78, 60,   45, 90]#放姿勢
servo_goal=      [118,40,  30, 130, 100,  120,   90]
servo_cal =      [0,  0,   0,   0,   0,    0,     0]


class Hand:
    def __init__(self, port, degree, goal):

        self.port = port
        self.degree = degree
        self.goal = goal


    def show_input(self):
        print("port,degree,goal,self.speed", self.port,self.degree,self.goal)

    pass
    ##復位為輸入角度
    def reset(self):
        servo.position(self.port, self.degree)
    ##角度誤差復位
    def action(self):
        pass
        if self.degree==self.goal:
            return(0)
        elif self.degree<self.goal:
            self.degree+=1
        elif self.degree>self.goal:
            self.degree-=1
        servo.position(self.port, self.degree)

def change_reset(reset):
    pass
    for i in range(7):
        servo_reset[i]=reset[i]



def readcode():
    global ballcode
    while(True):
        pass
        img = sensor.snapshot()
        img.lens_corr(1.8) # strength of 1.8 is good for the 2.8mm lens.
        for code in img.find_qrcodes():
            print(code)
            print('order is ',code[4][0],code[4][1],code[4][2])
            ballcode[0]=code[4][0]
            ballcode[1]=code[4][1]
            ballcode[2]=code[4][2]
            return(code[4])
            break

def changethreshold():
    pass
    global count
    global ballcode
    global ball_threshold
    print('the order is ',ballcode[0],ballcode[1],ballcode[2])
    print('count=',count)
    print('ballcode[count]=',ballcode[count])
    ballcode[count]=int(ballcode[count])
    if ballcode[count]==1:
        ball_threshold=red_threshold
        print('catch red ball for ',count+1,'times')
    elif ballcode[count]==2:
        ball_threshold=green_threshold
        print('catch green for ',count+1,'times')
    elif ballcode[count]==3:
        ball_threshold=blue_threshold
        print('catch blue for ',count+1,'times')






def change_goal(goal):
    pass
    servo0.goal=goal[0]
    servo1.goal=goal[1]
    servo2.goal=goal[2]
    servo3.goal=goal[3]
    servo4.goal=goal[4]
    servo5.goal=goal[5]
    servo6.goal=goal[6]


def handall_action():
    pass

    ser0=servo0.action()
    ser1=servo1.action()
    ser2=servo2.action()
    ser3=servo3.action()
    ser4=servo4.action()
    ser5=servo5.action()
    ser6=servo6.action()
    time.sleep(hand_speed)
    if ser0==0 and ser1==0 and ser2==0 and ser3==0 and ser4==0 and ser5==0 and ser6==0:
        return(0)

def hand6_action(goal):
    pass
    change_goal(goal)
    while(True):
        pass
        ser0=servo0.action()
        ser1=servo1.action()
        ser2=servo2.action()
        ser3=servo3.action()
        ser4=servo4.action()
        ser5=servo5.action()
        time.sleep(hand_speed)
        if ser0==0 and ser1==0 and ser2==0 and ser3==0 and ser4==0 and ser5==0 :
            break

def handaction(goal):
    change_goal(goal)
    while(True):
        pass
        ser0=servo0.action()
        ser1=servo1.action()
        ser2=servo2.action()
        ser3=servo3.action()
        ser4=servo4.action()
        ser5=servo5.action()
        ser6=servo6.action()
        time.sleep(hand_speed)
        if ser0==0 and ser1==0 and ser2==0 and ser3==0 and ser4==0 and ser5==0 and ser6==0:
            break
def ser6do(a):
    if 0<=servo6.goal<=180:
        if a<0:
            servo6.goal=servo6.goal+2
            print('ball turnleft')
        elif a>0:
            servo6.goal=servo6.goal-2
            print('ball turnright')
        servo6.action()
        time.sleep(10)
    print('ball servo6.goal=',servo6.goal)
    print('a=',a)

def find_ball():
    global k
    if 0<=servo6.goal<=160:
        if servo6.goal==0:
            k=1
        elif servo6.goal==160:
            k=-1
        if k==1:
             servo6.goal=servo6.goal+1
             #print('no ball turnleft')
        elif k==-1:
            servo6.goal=servo6.goal-1
            #print('no ball turnright')
        servo6.action()
        time.sleep(5)




def hand_hold():
    pass
    change_goal(servo_goal)
    while(1):
        ser0=servo0.action()
        ser1=servo1.action()
        time.sleep(10)
        if ser0==0 and ser1==0:
            break



def setreay():
    global count
    if ballcode[count]==1:
        ser6.goal=20
        print('set redball for ',count,'times')
    elif ballcode[count]==2:
        ser6.goal=30
        print('set greenball for ',count,'times')
    elif ballcode[count]==3:
        ser6.goal=40
        print('set blueball for ',count,'times')

    while(True):
        ser6=servo6.action()
        time.sleep(hand_speed)
        if ser6==0:
            break

def setball():
    print(0)

def catchball(x):
    pass
    pass
    
    a=x*x+110*110
    a=math.sqrt(a)
    xita=(-a*a+180*180+160*160)/(2*180*160)
    aerfa=(a*a+180*180-160*160)/(2*180*a)
    if -1<=xita<=1:
        xita=math.acos(xita)
        xita=int(math.degrees(xita))
    else:
        find_ball()
    if -1<=aerfa<=1:
        aerfa=math.acos(aerfa)
        aerfa=int(math.degrees(aerfa))
        print(aerfa)
        beta=180-aerfa-xita
        print(beta)
    else:
        find_ball()
        
    while(True):
        pass
        servo_cal[2]=90-beta-10
        servo_cal[4]=xita-26
        servo_cal[5]=aerfa+12
        servo2.goal=servo_cal[2]
        servo5.goal=servo_cal[5]
        servo4.goal=servo_cal[4]
        print("servo2.goal=",servo2.goal)
        print("servo4.goal=",servo4.goal)
        print("servo5.goal=",servo5.goal)
        ser2=servo2.action()
        ser4=servo4.action()
        ser5=servo5.action()
        time.sleep(10)
        if ser2==0 and ser4==0 and