# 多颜色跟踪示例
#
# 这个例子显示了使用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 ser5==0:
break
print('start......')
change_reset(servo_reset)
servo0=Hand(0,servo_reset[0],servo_goal[0])
servo1=Hand(2,servo_reset[1],servo_goal[1])
servo2=Hand(4,servo_reset[2],servo_goal[2])
servo3=Hand(6,servo_reset[3],servo_goal[3])
servo4=Hand(8,servo_reset[4],servo_goal[4])
servo5=Hand(10,servo_reset[5],servo_goal[5])
servo6=Hand(12,servo_reset[6],servo_goal[6])
servo0.reset()
servo1.reset()
servo2.reset()
servo3.reset()
servo4.reset()
servo5.reset()
servo6.reset()
time.sleep(2000)
K=21000
handaction(servo_camera)
readcode()
while(True):
pass
changethreshold()
time.sleep(3000)
handaction(servo_camera)
while(1):
clock.tick()
img = sensor.snapshot().lens_corr(1.8)
blobs = img.find_blobs([ball_threshold],pixels_threshold=400)
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[3]
length = int(K/Lm)
c_error=b[5]-160
c_range=20
if c_error<-c_range:
ser6do(-1)
elif c_error>c_range:
ser6do(1)
else:
break
#pass
print('c_error=',c_error)
#print('高像素=',b[3])
#print('距离length=',length)
else:
find_ball()
#print('no ball servo6.goal=',servo6.goal)
pass
while(1):
hand6_action(servo_up1)
catchball(150)
hand_hold()
hand6_action(servo_up2)
break
while(True):
count=count+1
if count==3:
while(1):
print('over!')
time.sleep(2000)
break
'''
while(1):
setreay()
#setball()
time.sleep(2000)
hand6_action(servo_open)
break
'''