April_tag individual test code

Code/OpenMV Code/Test_code/April_tag.py

+# 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