2023电赛E题激光打靶图像部分开源

方案:将摄像头中心与激光中心重合,通过矩形四角点坐标计算出边的斜率、直线方程等,进而规划出激光下一个理想坐标,方法如下

import sensor, image, time ,math,pyb
from pyb import Servo
from pid import PID
from pyb import UART

# 设置阈值用于二值化

threshold_val = (100, 255)  # 根据实际情况调整阈值
enable_lens_corr = False # turn on for straighter lines...打开以获得更直的线条…

#roi1 =(20, 20, 109, 85)
k=[0,0,0,0]
l1=3
point=[[0,0],[0,0],[0,0],[0,0]]
i=0
u=0
t1=0
t2=0
t3=0
t4=0
flag=0
flag_p=0

l2=0
l3=0
l4=0
xp=0
yp=0
k2=0
count=0
square1=[0,0,0,0,0,0,0]
square2=[0,0,0,0,0,0,0]
square3=[0,0,0,0,0,0,0]
square4=[0,0,0,0,0,0,0]
erro_x=0
erro_y=0
px1=0
py1=0
len1=0
sum=0

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)  # 设置图像格式为灰度
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time=2000)  # 等待摄像头稳定
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
uart = UART (3, 115200)

while True:
    img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.3)

    if enable_lens_corr: img.lens_corr(1.8)
    img.draw_circle( 80, 60 , 1, color = (255,0,0),fill=True)
    img.draw_circle( 80, 60 , 1, color = (255,0,0),fill=True)
    for r in img.find_rects(threshold = 30000):
        #img.draw_rectangle(r.rect(), color = (255, 0, 0))
        for p in r.corners():

            #img.binary([threshold_val])
            #if((point[i][0]<=0) or (point[i][1]<=0) or (point[i][0]>=159) or (point[i][1]>=159)):
                #point[i][0]=0
                #point[i][1]=0

            #else:
                #point[i][0]=p[0]
                #point[i][1]=p[1]
            point[i][0]=p[0]
            point[i][1]=p[1]

            if(i==3):

                img.draw_circle(p[0], p[1], 3, color = (155, 0, 255),fill=True)
            if(i!=3):
                img.draw_circle(p[0], p[1], 2, color = (0, 0, 255),fill=True)

            if (point[3][0]>0):
                k[i]=((point[(i+1)%4][1]-point[i][1])/(point[(i+1)%4][0]-point[i][0]+0.0001))
            if(i==1):
                point[i][0]=point[i][0]-3
                point[i][1]=point[i][1]-3
            if(i==2):
                point[i][0]=point[i][0]-3
                point[i][1]=point[i][1]+3
            if(i==3):
                point[i][0]=point[i][0]+4
                point[i][1]=point[i][1]+4
            if(i==0):
                point[i][0]=point[i][0]
                point[i][1]=point[i][1]

                #point[i][0]=p[0]-4/(math.sqrt(1+k[2]*k[2]))
                #point[i][1]=p[1]-(4*k[i])/(math.sqrt(1+k[0]*k[0]))

            i=i+1
            if (i == 4) :
                i=0


   # print(k[0],k[1],k[2],k[3])
    if(flag==0 and point[0][0]>0 and point[0][1]>0):
        erro_x=80-point[3][0]
        erro_y=60-point[3][1]
        print(point[3][0]*1.0,point[3][1]*1.0)
        uart.write('%')
        uart.write(str(len(str(point[3][0]*1.0))))
        uart.write(str(point[3][0]*1.0))
        uart.write(str(len(str(point[3][1]*1.0))))
        uart.write(str(point[3][1]*1.0))
        uart.write('&')


    if(flag==0 and math.sqrt(erro_x*erro_x+erro_y*erro_y)<1):
        flag=1##循迹开始
        erro_x=0
        erro_y=0


     #   3   k2   2
     #   ----------
     #   |        |
     #   |        |
     #k3 |        | k1
     #   |        |
     #   |        |
     #   |        |
     #   ----------
     #   0   k0   1

    if(flag==1):#循迹部分
        erro_x=80-point[2][0]
        erro_y=60-point[2][1]
        #if(math.sqrt(erro_x*erro_x+erro_y*erro_y)>0):
        #xp1=-1*(point[3][0]-80)
        #yp1=-1*(point[3][1]-60)
        #xp=(l1/math.sqrt(1+k[2]*k[2]))+(k[2]*k[2]*xp1-yp1*k[2])/(1+k[2]*k[2])
        #yp=(l1*k[2])/math.sqrt(1+k[2]*k[2])+(yp1-k[2]*xp1)/(1+k[2]*k[2])



        xp1=k[2]*k[2]+1

        yp=((80*k[2]+(60-point[3][1])*(xp1-1)-point[3][0]*k[2])/xp1+point[3][1])+(l1*k[2])/math.sqrt(xp1)

        xp=(((xp1-1)*point[3][0]+80+(60-point[3][1])*k[2])/xp1)+(l1)/math.sqrt(xp1)

        img.draw_circle( int(xp), int(yp) , 2, color = (0,255,0),fill=True)


        img.draw_line((int(point[3][0]),int(point[3][1]),int(point[2][0]),int(point[2][1])), color = (255,255,0))
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)>10):
            print(round(xp,1),round(yp,1))
            uart.write('%')

            uart.write(str(len(str(round(xp,1)))))
            uart.write(str(round(xp,1)))
            uart.write(str(len(str(round(yp,1)))))
            uart.write(str(round(yp,1)))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<=10 and math.sqrt(erro_x*erro_x+erro_y*erro_y)>=2):
            print(point[2][0]*1.0,point[2][1]*1.0)
            uart.write('%')
            uart.write(str(len(str(point[2][0]*1.0))))
            uart.write(str(point[2][0]*1.0))
            uart.write(str(len(str(point[2][1]*1.0))))
            uart.write(str(point[2][1]*1.0))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<2):
            flag=2
            erro_x=0
            erro_y=0

    if(flag==2):#循迹部分
        erro_x=80-point[1][0]
        erro_y=60-point[1][1]
        xp1=k[1]*k[1]+1
        if(k[1]>=0):

            yp=((80*k[1]+(60-point[2][1])*(xp1-1)-point[2][0]*k[1])/xp1+point[2][1])+(l1*k[1])/math.sqrt(xp1)

            xp=(((xp1-1)*point[2][0]+80+(60-point[2][1])*k[1])/xp1)+(l1)/math.sqrt(xp1)
        if(k[1]<0):
            yp=((80*k[1]+(60-point[2][1])*(xp1-1)-point[2][0]*(k[1]))/xp1+point[2][1])-(l1*(k[1]))/math.sqrt(xp1)

            xp=(((xp1-1)*point[2][0]+80+(60-point[2][1])*k[1])/xp1)-(l1)/math.sqrt(xp1)
        img.draw_circle( int(xp), int(yp) , 2, color = (255,255,0),fill=True)

        img.draw_line((int(point[1][0]),int(point[1][1]),int(point[2][0]),int(point[2][1])), color = (255,255,0))

        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)>10):
            print(round(xp,1),round(yp,1))
            uart.write('%')

            uart.write(str(len(str(round(xp,1)))))
            uart.write(str(round(xp,1)))
            uart.write(str(len(str(round(yp,1)))))
            uart.write(str(round(yp,1)))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<=10 and math.sqrt(erro_x*erro_x+erro_y*erro_y)>=2):
            print(point[1][0]*1.0,point[1][1]*1.0)
            uart.write('%')
            uart.write(str(len(str(point[1][0]*1.0))))
            uart.write(str(point[1][0]*1.0))
            uart.write(str(len(str(point[1][1]*1.0))))
            uart.write(str(point[1][1]*1.0))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<2):
            flag=3
            erro_x=0
            erro_y=0

    if(flag==3):#循迹部分
        erro_x=80-point[0][0]
        erro_y=60-point[0][1]
        xp1=k[0]*k[0]+1

        yp=((80*k[0]+(60-point[1][1])*(xp1-1)-point[1][0]*k[0])/xp1+point[1][1])-(l1*k[0])/math.sqrt(xp1)

        xp=(((xp1-1)*point[1][0]+80+(60-point[1][1])*k[0])/xp1)-(l1)/math.sqrt(xp1)
        img.draw_circle( int(xp), int(yp) , 2, color = (255,255,0),fill=True)

        img.draw_line((int(point[1][0]),int(point[1][1]),int(point[0][0]),int(point[0][1])), color = (255,255,0))

        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)>10):
            print(round(xp,1),round(yp,1))
            uart.write('%')

            uart.write(str(len(str(round(xp,1)))))
            uart.write(str(round(xp,1)))
            uart.write(str(len(str(round(yp,1)))))
            uart.write(str(round(yp,1)))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<=10 and math.sqrt(erro_x*erro_x+erro_y*erro_y)>=2):
            print(point[0][0]*1.0,point[0][1]*1.0)
            uart.write('%')
            uart.write(str(len(str(point[0][0]*1.0))))
            uart.write(str(point[0][0]*1.0))
            uart.write(str(len(str(point[0][1]*1.0))))
            uart.write(str(point[0][1]*1.0))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<2):
            flag=4
            erro_x=0
            erro_y=0

    if(flag==4):#循迹部分
        erro_x=80-point[3][0]
        erro_y=60-point[3][1]
        xp1=k[3]*k[3]+1
        if(k[3]>=0):
            yp=((80*k[3]+(60-point[0][1])*(xp1-1)-point[0][0]*k[3])/xp1+point[0][1])-(l1*k[3])/math.sqrt(xp1)

            xp=(((xp1-1)*point[0][0]+80+(60-point[0][1])*k[3])/xp1)-(l1)/math.sqrt(xp1)
        if(k[3]<0):
            yp=((80*k[3]+(60-point[0][1])*(xp1-1)-point[0][0]*(k[3]))/xp1+point[0][1])+(l1*(k[3]))/math.sqrt(xp1)

            xp=(((xp1-1)*point[0][0]+80+(60-point[0][1])*k[3])/xp1)+(l1)/math.sqrt(xp1)
        img.draw_circle( int(xp), int(yp) , 2, color = (255,255,0),fill=True)


        img.draw_line((int(point[3][0]),int(point[3][1]),int(point[0][0]),int(point[0][1])), color = (255,101,255))

        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)>10):
            print(round(xp,1),round(yp,1))
            uart.write('%')

            uart.write(str(len(str(round(xp,1)))))
            uart.write(str(round(xp,1)))
            uart.write(str(len(str(round(yp,1)))))
            uart.write(str(round(yp,1)))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<=10 and math.sqrt(erro_x*erro_x+erro_y*erro_y)>=2):
            print(point[3][0]*1.0,point[3][1]*1.0)

            uart.write('%')
            uart.write(str(len(str(point[3][0]*1.0))))
            uart.write(str(point[3][0]*1.0))
            uart.write(str(len(str(point[3][1]*1.0))))
            uart.write(str(point[3][1]*1.0))
            uart.write('&')
        if(math.sqrt(erro_x*erro_x+erro_y*erro_y)<2):
            flag=5
            erro_x=0
            erro_y=0


    if(flag==5):#循迹部分
            print('end!')
            uart.write('%')
            uart.write('4')
            uart.write('80.0')
            uart.write('4')
            uart.write('60.0')
            uart.write('&')

            img.draw_string(5,17,"CYT,YOU ARE SOOO HANDSOME",color = (255, 0, 255), scale=1.1,x_spacing=-2)








######################################################################################
    #print(point[0][0],point[0][1])
    #print(point[1][0],point[1][1])
    #print(point[2][0],point[2][1])
    #print(point[3][0],point[3][1])
    #print(k[0],k[1],k[2],k[3])




    #square1=[img.get_pixel(point[0][0]+1,point[0][1]+1),img.get_pixel(point[0][0]-1,point[0][1]-1),img.get_pixel(point[0][0],point[0][1]-1),img.get_pixel(point[0][0]-1,point[0][1]),
    #img.get_pixel(point[0][0]+1,point[0][1]),img.get_pixel(point[0][0],point[0][1]+1),img.get_pixel(point[0][0],point[0][1])]
    #square2=[img.get_pixel(point[1][0]+1,point[1][1]+1),img.get_pixel(point[1][0]-1,point[1][1]-1),img.get_pixel(point[1][0],point[1][1]-1),img.get_pixel(point[1][0]-1,point[1][1]),
    #img.get_pixel(point[1][0]+1,point[1][1]),img.get_pixel(point[1][0],point[1][1]+1),img.get_pixel(point[1][0],point[1][1])]
    #square3=[img.get_pixel(point[2][0]+1,point[2][1]+1),img.get_pixel(point[2][0]-1,point[2][1]-1),img.get_pixel(point[2][0],point[2][1]-1),img.get_pixel(point[2][0]-1,point[2][1]),
    #img.get_pixel(point[2][0]+1,point[2][1]),img.get_pixel(point[2][0],point[2][1]+1),img.get_pixel(point[2][0],point[2][1])]
    #square4=[img.get_pixel(point[3][0]+1,point[3][1]+1),img.get_pixel(point[3][0]-1,point[3][1]-1),img.get_pixel(point[3][0],point[3][1]-1),img.get_pixel(point[3][0]-1,point[3][1]),
    #img.get_pixel(point[3][0]+1,point[3][1]),img.get_pixel(point[3][0],point[3][1]+1),img.get_pixel(point[3][0],point[3][1])]
    #if(k[3]>0):
        #count=count+1
        #if(count>200):
            #t1=((square1[0]+square1[1]+square1[2]+square1[3]+square1[4]+square1[5]+3*square1[6])/255)
            #t2=((square2[0]+square2[1]+square2[2]+square2[3]+square2[4]+square2[5]+3*square2[6])/255)
            #t3=((square3[0]+square3[1]+square3[2]+square3[3]+square3[4]+square3[5]+3*square3[6])/255)
            #t4=((square4[0]+square4[1]+square4[2]+square4[3]+square4[4]+square4[5]+3*square4[6])/255)
            #count=205
    #print(square1[0],square1[1],square1[2],square1[3],count)
    #print(point[0][0],point[0][1])
    #print(point[1][0],point[1][1])
    #print(point[2][0],point[2][1])
    #print(point[3][0],point[3][1])

效果如下

若有错漏,欢迎指正

  • 3
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值