方案:将摄像头中心与激光中心重合,通过矩形四角点坐标计算出边的斜率、直线方程等,进而规划出激光下一个理想坐标,方法如下
![](https://i-blog.csdnimg.cn/blog_migrate/35a8d8f4285663c36fa1995c93338062.jpeg)
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])
效果如下
若有错漏,欢迎指正