附上自己写的源代码(后续补充本文)

# © Copyright By Digran, 2020 November 1st, in NJUST.
# This code is used in the Optoelectronic Design Competition.
# This code is used for color recognition and shape recognition.
import sensor, image, time, math
from pyb import UART
import json

#Set the tuple corresponding to the color threshold of each block.
red_threshold  = [(55, 71, -128, 127, 23, 127)]
blue_threshold = [(35, 95, -100, 38, -45, -102)]
green_threshold = [(72, 100, -118, -42, -128, 127)]
gray_threshold = [(100, 76, -128, 127, 20, 127)]
#Set the area of interest of the Cam3.
ROI = (80, 60, 160, 120)

#A series of operations such as camera initialization.
sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()

#Set the serial port number and baud rate of the serial port.
uart = UART(3, 115200)


while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)
    #statistics = img.get_statistics(roi=ROI)

    red_blobs = img.find_blobs(red_threshold, pixels_threshold=300, area_threshold=1200, merge = True, margin = 10)
    blue_blobs = img.find_blobs(blue_threshold, pixels_threshold=300, area_threshold=1200, merge = True, margin = 10)
    green_blobs = img.find_blobs(green_threshold, pixels_threshold=300, area_threshold=1200, merge = True, margin = 10)
    gray_blobs = img.find_blobs(gray_threshold, pixels_threshold=300, area_threshold=1200, merge = True, margin = 10)

    if red_blobs:
        for red in red_blobs:
            img.draw_rectangle(red.rect(), color = (255, 255, 255))
            uart.write("0x01")
            print("0x01")

    if blue_blobs:
        for blue in blue_blobs:
            img.draw_rectangle(blue.rect(), color = (255, 255, 255))
            uart.write("0x02")
            print("0x02")

    if green_blobs:
        for green in green_blobs:
            img.draw_rectangle(green.rect(), color = (255, 255, 255))
            uart.write("0x03")
            print("0x03")

    if gray_blobs:
        for gray in gray_blobs:
            img.draw_rectangle(gray.rect(), color = (255, 255, 255))
            uart.write("0x04")
            print("0x04")
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
Last modification:November 18th, 2020 at 10:15 pm
小编饿得写不动了,请给小编加鸡腿