diff --git a/Code/OpenMV Code/Test_code/April_tag.py b/Code/OpenMV Code/Test_code/April_tag.py new file mode 100644 index 0000000000000000000000000000000000000000..df9f5eb903f49736aeefb17dd9f9a83251be74a5 --- /dev/null +++ b/Code/OpenMV Code/Test_code/April_tag.py @@ -0,0 +1,84 @@ +# Untitled - By: polar - Wed Oct 6 2021 + +import sensor, image, time + +import image, network, math, rpc, sensor, struct, tf, ucollections, pyb +#from pyb import UART, +import ubinascii + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.QVGA) +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... +sensor.skip_frames(time = 2000) + +red_led = pyb.LED(1) +green_led = pyb.LED(2) +blue_led = pyb.LED(3) +red_led.off() +green_led.off() +blue_led.off() #put the LEDs into a known state +red_led.on() #turn the LED to red so that we know it is on + +interface = rpc.rpc_i2c_slave(slave_addr=0x12) + +#Constants needed +MAX_BLOBS = 4 +TAG_SIZE = 138 #length of inner black border of tag in mm (if printing to fill normal paper, = 138mm) +MAX_TAGS = 2 +XRES = 320 +YRES = 240 +SIZE = 16.3 +f_x = (2.8 / 3.673) * XRES # find_apriltags defaults to this if not set 3.984 +f_y = (2.8 / 2.738) * YRES # find_apriltags defaults to this if not set +c_x = XRES * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5) +c_y = YRES * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5) + +def apriltags(): + red_led.off() + green_led.on() + sensor.set_pixformat(sensor.RGB565) + sensor.set_framesize(sensor.QQVGA) + 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... + #need to hardcode tag size for each ID + LENS_MM = 2.8 + LENS_TO_CAM_MM = 22 + + # datas = struct.unpack("<bb", data) + # id1 = datas[0] + id1 = 1 + #add more options based on what tag size will actually be + #this is default size for now + tagsize1 = 157 + if id1 == 0: + tagsize1 = 138 + #sensor.set_pixformat(sensor.GRAYSCALE) + #sensor.set_framesize(sensor.QVGA) # we run out of memory if the resolution is much bigger... + #sensor.set_windowing((X_RES, Y_RES)) # Look at center 160x120 pixels of the VGA resolution. + img = sensor.snapshot() + + f_x = (LENS_MM / 3.984) * X_RES # find_apriltags defaults to this if not set + f_y = (LENS_MM / 2.952) * Y_RES # find_apriltags defaults to this if not set + c_x = X_RES * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5) + c_y = Y_RES * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5) + + tags = [0, 0, 0] + #n_tags = 0 + for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11 + img.draw_rectangle(tag.rect(), color = (255, 0, 0)) + img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) + dist = dist_conversion(tag.z_translation()) + img.draw_string(tag.cx(), tag.cy(), str(dist)) + if tag.id() == id1: + print(tag.id()) + TAG_SIZE = tagsize1 + tags[0:2] = [int(tag.cx()), int(tag.cy()), int(degrees(tag.z_rotation()))] + red_led.on() + green_led.off() + return struct.pack("<hhh", tags[0], tags[1], tags[2]) + +if __name__ == '__main__': + while True: + apriltags() \ No newline at end of file