diff --git a/Code/OpenMV Code/goalfinder.py b/Code/OpenMV Code/goalfinder.py new file mode 100644 index 0000000000000000000000000000000000000000..3c1917c918b84631c245a43316c9f168e70f0f37 --- /dev/null +++ b/Code/OpenMV Code/goalfinder.py @@ -0,0 +1,85 @@ +# 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())