# Untitled - By: Administrator - 周二 12月 8 2020
# AprilTags Example
#
# This example shows the power of the OpenMV Cam to detect April Tags
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
import sensor, image, time, math,network, usocket, sys, json
SSID ='OPENMV_AP' # Network SSID
KEY ='1234567890' # Network key (must be 10 chars)
HOST = '' # Use first available interface
PORT = 8080 # Arbitrary non-privileged port
green_threshold = ( 0, 80, -70, -10, -0, 30)
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()
# 注意！与find_qrcodes不同，find_apriltags 不需要软件矫正畸变就可以工作。
# 注意，输出的姿态的单位是弧度，可以转换成角度，但是位置的单位是和你的大小有关，需要等比例换算
# f_x 是x的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/3.984*656，这个值是用毫米为单位的焦距除以x方向的感光元件的长度，乘以x方向的感光元件的像素（OV7725）
# f_y 是y的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/2.952*488，这个值是用毫米为单位的焦距除以y方向的感光元件的长度，乘以y方向的感光元件的像素（OV7725）
# c_x 是图像的x中心位置
# c_y 是图像的y中心位置
# Init wlan module in AP mode.
wlan = network.WINC()
wlan.connect(SSID, key=KEY, security=wlan.WPA_PSK)
#wlan.start_ap(SSID, key=KEY, security=wlan.WEP, channel=2)
# You can block waiting for client to connect
#print(wlan.wait_for_sta(10000))
def response(s):
print ('Waiting for connections..')
client, addr = s.accept()
# set client socket timeout to 2s
client.settimeout(2.0)
print ('Connected to ' + addr[0] + ':' + str(addr[1]))
# Read request from client
data = client.recv(1024)
# Should parse client request here
# Send multipart header
client.send("HTTP/1.1 200 OK\r\n" \
"Server: OpenMV\r\n" \
"Content-Type: application/json\r\n" \
"Cache-Control: no-cache\r\n" \
"Pragma: no-cache\r\n\r\n")
# FPS clock
clock = time.clock()
client.send(dx)
client.close()
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)
def degrees(radians):
return (180 * radians) / math.pi
while(True):
s = usocket.socket(usocket.AF_INET, usocket.SOCK_STREAM)
try:
# Bind and listen
s.bind([HOST, PORT])
s.listen(5)
# Set server socket timeout
# NOTE: Due to a WINC FW bug, the server socket must be closed and reopened if
# the client disconnects. Use a timeout here to close and re-create the socket.
s.settimeout(3)
response(s)
except OSError as e:
s.close()
print("socket error: ", e)
#sys.print_exception(e)
clock.tick()
img = sensor.snapshot()
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # 默认为TAG36H11
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
d=dx=dy=dz=1.0
Tx=abs(tag.x_translation())
Ty=abs(tag.y_translation())
Tz=abs(tag.z_translation())
dx=27.33*Tx
dy=27.33*Ty
dz=27.33*Tz
print_args = (dx, dy, dz, \
degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
# 位置的单位是未知的，旋转的单位是角度
print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
import requests
dx = requests.get('192.168.1.1:8080')
dy = requests.get('192.168.1.1:8080')
dz = requests.get('192.168.1.1:8080')
