Upload New File

Code/OpenMV Code/goalfinder.py

+# 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
+
+sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QVGA) # we run out of memory if the resolution is much bigger...
+sensor.skip_frames(time = 2000)
+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()
+XRES = 320
+YRES = 240
+
+# Note! Unlike find_qrcodes the find_apriltags method does not need lens correction on the image to work.
+
+# What's the difference between tag families? Well, for example, the TAG16H5 family is effectively
+# a 4x4 square tag. So, this means it can be seen at a longer distance than a TAG36H11 tag which
+# is a 6x6 square tag. However, the lower H value (H5 versus H11) means that the false positve
+# rate for the 4x4 tag is much, much, much, higher than the 6x6 tag. So, unless you have a
+# reason to use the other tags families just use TAG36H11 which is the default family.
+
+# The AprilTags library outputs the pose information for tags. This is the x/y/z translation and
+# x/y/z rotation. The x/y/z rotation is in radians and can be converted to degrees. As for
+# translation the units are dimensionless and you must apply a conversion function.
+
+# f_x is the x focal length of the camera. It should be equal to the lens focal length in mm
+# divided by the x sensor size in mm times the number of pixels in the image.
+# The below values are for the OV7725 camera with a 2.8 mm lens.
+
+# f_y is the y focal length of the camera. It should be equal to the lens focal length in mm
+# divided by the y sensor size in mm times the number of pixels in the image.
+# The below values are for the OV7725 camera with a 2.8 mm lens.
+
+# c_x is the image x center position in pixels.
+# c_y is the image y center position in pixels.
+
+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 degrees(radians):
+    return (180 * radians) / math.pi
+
+#val = value from apriltag reading
+#size = apriltag square length in cm
+def ATDistance(val,size):
+    new_val = -10*val*size/16.3
+    return new_val
+
+SIZE = 3.5
+goal_id = [0, 1, 2]
+
+while(True):
+    nearest_tag = [0,0,0,float('inf'),0,0,0]
+    clock.tick()
+    img = sensor.snapshot()
+    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))
+        print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(), \
+            degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
+        # Translation units are unknown. Rotation units are in degrees.
+        #print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
+        #print("distance: ", ATDistance(tag.z_translation(),SIZE), " cm. ID: ",tag.id() )
+        for i in goal_id:
+            if tag.id() == i and ATDistance(tag.z_translation(),SIZE) < nearest_tag[3]:
+                nearest_tag[0] = tag.id()
+                nearest_tag[1] = tag.x_translation()
+                nearest_tag[2] = tag.y_translation()
+                nearest_tag[3] = ATDistance(tag.z_translation(),SIZE)
+                nearest_tag[4] = tag.x_rotation()
+                nearest_tag[5] = tag.y_rotation()
+                nearest_tag[6] = tag.z_rotation()
+    #check to see if there's a tag in frame
+    if nearest_tag[3] == float('inf'):
+        print("no tags in sight!")
+    else:
+        print("nearest tag id is ", nearest_tag[0], " with the distance", nearest_tag[3], " cm.")
+
+    #print(clock.fps())