diff --git a/Code/OpenMV Code/Test_code/April_tag.py b/Code/OpenMV Code/Test_code/April_tag.py index df9f5eb903f49736aeefb17dd9f9a83251be74a5..54ac4fb81cc1c061968fbd1e152c6eb49bf2892b 100644 --- a/Code/OpenMV Code/Test_code/April_tag.py +++ b/Code/OpenMV Code/Test_code/April_tag.py @@ -1,6 +1,3 @@ -# 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, @@ -29,56 +26,92 @@ TAG_SIZE = 138 #length of inner black border of tag in mm (if printing to fill n MAX_TAGS = 2 XRES = 320 YRES = 240 -SIZE = 16.3 +SIZE = 15.8 # unit: cm 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() +#################### constants and functions for AprilTags #################### + + +def change_tag_size(size): + TAG_SIZE = size + #return size + +def degrees(radians): + return (180 * radians) / math.pi + +def dist_conversion(z): + #returns the distance in mm + z = z*100*2 + scale = TAG_SIZE/138 + res_scale = (240/X_RES + 240/Y_RES)/2 + return z*scale*res_scale + +def xy_to_mm(tag1_cx, tag1_cy, tag2_cx, tag2_cy, tag3_cx, tag3_cy): + #find the actual distance between tag1 and tag3 + mm_distx = 590 #x dist in mm between two tags (premeasured) + mm_disty = 1 #y dist ^ (lol can't be 0 or the eqn breaks) + pixel_distx = tag1_cx - tag2_cx #dist between two tags cx (in pixels) + pixel_disty = tag1_cy - tag2_cy #same but cy + target_distx = tag1_cx - tag3_cx #dist between tag1 cx and object cx + target_disty = tag1_cy - tag3_cy #same but cy + #unless that shit is constant or passed in, yikes + #the 2.8 is a scaling factor based on lens size of cam + distx = target_distx*mm_distx/pixel_distx/2.8 + disty = target_disty*mm_disty/pixel_disty/2.8 + return (distx, disty) #in mm + +#returns a list of the x,y centroid, distance, and rotation for 2 tags of a certain ID + + +def ATDistance(val,size): + #val = value from apriltag reading + #size = apriltag square length in cm + + new_val = -10*val*size/16.3 + return new_val + +def goalfinder(data): sensor.set_pixformat(sensor.RGB565) - sensor.set_framesize(sensor.QQVGA) + sensor.set_framesize(sensor.QVGA) # we run out of memory if the resolution is much bigger... 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. + # goal_id = struct.unpack("<bbb", data) + goal_id = data + nearest_tag = [0,0,0,float('inf'),0,0,0] 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 + img.draw_cross(160, 120, color = (255, 0, 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 + + 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[1] = tag.cx() + nearest_tag[2] = tag.cy() + 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'): + # return struct.pack("<hhhhhhh", nearest_tag) + print(nearest_tag) + else: + print("nearest tag id is ", nearest_tag[0], " with the distance", nearest_tag[3], " cm.") + print("position x,y:",tag.x_translation(),tag.y_translation()) + print("position x,y:",tag.cx(), tag.cy()) + # return struct.pack("<hhhhhhh", nearest_tag) + + +# test + +while True: + data = [0,1,2] + + goalfinder(data) diff --git a/Code/OpenMV Code/Test_code/April_tag_with_CtlBd.py b/Code/OpenMV Code/Test_code/April_tag_with_CtlBd.py new file mode 100644 index 0000000000000000000000000000000000000000..4af523e6b748821d2f7b97ecff9eebcc59d29122 --- /dev/null +++ b/Code/OpenMV Code/Test_code/April_tag_with_CtlBd.py @@ -0,0 +1,122 @@ + +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 = 15.8 # unit: cm +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) + +#################### constants and functions for AprilTags #################### + + +def change_tag_size(size): + TAG_SIZE = size + #return size + +def degrees(radians): + return (180 * radians) / math.pi + +def dist_conversion(z): + #returns the distance in mm + z = z*100*2 + scale = TAG_SIZE/138 + res_scale = (240/X_RES + 240/Y_RES)/2 + return z*scale*res_scale + +def xy_to_mm(tag1_cx, tag1_cy, tag2_cx, tag2_cy, tag3_cx, tag3_cy): + #find the actual distance between tag1 and tag3 + mm_distx = 590 #x dist in mm between two tags (premeasured) + mm_disty = 1 #y dist ^ (lol can't be 0 or the eqn breaks) + pixel_distx = tag1_cx - tag2_cx #dist between two tags cx (in pixels) + pixel_disty = tag1_cy - tag2_cy #same but cy + target_distx = tag1_cx - tag3_cx #dist between tag1 cx and object cx + target_disty = tag1_cy - tag3_cy #same but cy + #unless that shit is constant or passed in, yikes + #the 2.8 is a scaling factor based on lens size of cam + distx = target_distx*mm_distx/pixel_distx/2.8 + disty = target_disty*mm_disty/pixel_disty/2.8 + return (distx, disty) #in mm + +#returns a list of the x,y centroid, distance, and rotation for 2 tags of a certain ID + + +def ATDistance(val,size): + #val = value from apriltag reading + #size = apriltag square length in cm + + new_val = -10*val*size/16.3 + return new_val + +def goalfinder(data): + sensor.set_pixformat(sensor.RGB565) + sensor.set_framesize(sensor.QVGA) # we run out of memory if the resolution is much bigger... + 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... + # goal_id = struct.unpack("<bbb", data) + goal_id = data + nearest_tag = [0,0,0,float('inf'),0,0,0] + img = sensor.snapshot() + img.draw_cross(160, 120, color = (255, 0, 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)) + + 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[1] = tag.cx() + nearest_tag[2] = tag.cy() + 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 + return struct.pack("<hhhhhhh", nearest_tag) + """ + if nearest_tag[3] == float('inf'): + print(nearest_tag) + else: + print("nearest tag id is ", nearest_tag[0], " with the distance", nearest_tag[3], " cm.") + print("position x,y:",tag.x_translation(),tag.y_translation()) + print("position x,y:",tag.cx(), tag.cy()) + """ + + + +# Register call backs. + +interface.register_callback(goalfinder) + +# Once all call backs have been registered we can start +# processing remote events. interface.loop() does not return. + +interface.loop() +