import sensor, image, time, math,pyb
from pyb import UART
uart=UART(3,1382400)#其中3为总线固定值,9600为波特率可改
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(30)
sensor.set_auto_gain(False) # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False) # must turn this off to prevent image washout...
clock = time.clock()
##########数据初始化#########![0_1578749067208_{FMS(STBPMYV6@Y1H)$534P.png](https://fcdn.singtown.com/7a0ed25a-d449-4d47-9517-b2ec1bd51358.png)
f_x = (2.8 / 3.984) * 160 # 默认值
f_y = (2.8 / 2.952) * 120 # 默认值
c_x = 160 * 0.5 # 默认值(image.w * 0.5)
c_y = 120 * 0.5 # 默认值(image.h * 0.5)
k=10.638
oldx=0
oldy=0
oldyh=0
oldyl=0
oldz=0
oldm=0
oldn=0
x=0
y=0
yh=0
yl=0
z=0
m=0
n=0
#####数据初始话结束#############
led_b = pyb.LED(3) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
led_b.on ()
def degrees(radians):
return (180 * radians) / math.pi
def find_max(tags):
max_id=0
for apriltag in tags:
if apriltag.id() > max_id:
max_apriltag=apriltag
max_id = apriltag.id()
return max_apriltag
while(True):
clock.tick()
led_g = pyb.LED(2) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
img = sensor.snapshot()
tags = img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y) # 默认为TAG36H11
if tags:
find_picture=1
else:
find_picture=0
if find_picture:
led_g.on()
FH = bytearray([0xb3,0xb3])
uart.write(FH)
tag=find_max(tags)
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
zt=tag.z_translation()
xt=tag.x_translation()
yt=tag.y_translation()
length =k*math.sqrt(zt*zt+xt*xt+yt*yt)
x=int(length)
y=int(degrees(tag.y_rotation()))
yh=int(y/100)
yl=int(y%100)
z=tag.cx()
m=tag.cy()
n=tag.id()
oldx=x
oldy=y
oldyh=yh
oldyl=yl
oldz=z
oldm=m
oldn=n
data = bytearray([z,m,n,x,yh,yl])
uart.write(data)
print('x=',z,'y=',m,'length=',length,'ID=',tag.id())
print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(),\
degrees(tag.x_rotation()), yh,yl, degrees(tag.z_rotation()))
# 位置的单位是未知的,旋转的单位是角度
print("Tx: %f, Ty %f, Tz %f, Rx %f, Ryh %f,Ryl %f, Rz %f" % print_args)
print('find_picture=',find_picture)
else:
led_g.off()
FH = bytearray([0xb3,0xb3])
uart.write(FH)
#img.draw_rectangle(tag.rect(), color = (255, 0, 0))
#img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
data = bytearray([oldz,oldm,oldn,oldx,oldyh,oldyl])
uart.write(data)
print('x=',oldz,'y=',oldm,'ID=',oldn,'length=',oldx,'yh=',yh,'yl=',yl,'find_picture=',find_picture)
print('find_picture=',find_picture)
#print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(),\
#degrees(tag.x_rotation()),yh,yl, degrees(tag.z_rotation()))
# 位置的单位是未知的,旋转的单位是角度
#print("Tx: %f, Ty %f, Tz %f, Rx %f, Ryh %f,Ryl %f, Rz %f" % print_args)
return max_apriltag这里报错有大佬能帮一下吗,完全懵了