def update_contour():
"""
Finds the largest orange contour and use it to update contour_center
"""
global contour_center
image = rc.camera.get_color_image()
if image is None:
contour_center = None
else:
# Find all of the orange contours
contours = rc_utils.find_contours(image, ORANGE[0], ORANGE[1])
# Select the largest contour
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
else:
contour_center = None
# Display the image to the screen
rc.display.show_color_image(image)
def updateContour(self, rc, depth_image, color_image):
if color_image is None:
self.contour_center = None
else:
# Crop the image to the floor directly in front of the car
contour_image = rc_utils.crop(color_image, c.LINE_CROP_FLOOR[0],
c.LINE_CROP_FLOOR[1])
contours = rc_utils.find_contours(contour_image,
self.color.value[0],
self.color.value[1])
L_contour = rc_utils.get_largest_contour(contours,
c.LINE_MIN_CONTOUR_AREA)
if L_contour is not None:
self.contour_center = rc_utils.get_contour_center(L_contour)
contour_area = rc_utils.get_contour_area(L_contour)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, L_contour, (0, 255, 255))
rc_utils.draw_circle(contour_image, self.contour_center,
(0, 255, 255))
def run_fast(self, rc, dist_slow):
# Get 2 largest fast contours
largest = self.get_2_largest(self.cropped_img, self.fast_col)
if len(largest) == 2:
# Finds which contour is which (left side/ right side)
avg_1 = np.average(largest[0][1][:, :, 0])
avg_2 = np.average(largest[1][1][:, :, 0])
avg_mid = (avg_2 + avg_1) / 2
if avg_2 > avg_1:
right_cont = largest[1][1]
left_cont = largest[0][1]
else:
right_cont = largest[0][1]
left_cont = largest[1][1]
# Sets angle based on mid average offset from center
self.angle = rc_utils.remap_range(
avg_mid - (rc.camera.get_width() / 2), *c.ALIGN_REMAP)
# <>>>>>> VISZHUALS
rc_utils.draw_contour(self.cropped_img,
left_cont,
color=(255, 0, 0))
rc_utils.draw_contour(self.cropped_img,
right_cont,
color=(0, 255, 0))
cv.line(self.cropped_img, (int(avg_mid), 0),
(int(avg_mid), rc.camera.get_height()), (255, 255, 0))
elif len(largest) == 1:
# If the center of the contour is roughly in the center of the screen, tell car to turn
offset = abs(
rc_utils.get_contour_center(largest[0][1])[1] -
(rc.camera.get_width() // 2))
if offset < c.LANE_SPLIT_DETECT_MAX_OFFSET:
self.angle = 1
# Sets speed based on distance from sharp turn and based on angle
speed_approach_remap = rc_utils.remap_range(dist_slow,
*c.SLOWDOWN_REMAP)
speed_angle_remap = rc_utils.remap_range(abs(self.angle),
*c.SPEED_ANGLE_REMAP)
speed = min((speed_angle_remap, speed_approach_remap))
# Checks ramp if y accel is below a threshold
if rc.physics.get_linear_acceleration()[1] >= -9.59:
speed = 1
# Sets speed and angle
rc.drive.set_speed_angle(rc_utils.clamp(speed, -1, 1),
rc_utils.clamp(self.angle, -1, 1))
def update_ar(info, info_id, ar_image):
global FIRST_PRI1
global SECOND_PRI1
global red_dir
global blue_dir
global green_dir
red_dir = 0
blue_dir = 0
green_dir = 0
#ar_image= rc_utils.crop(image, (0, rc.camera.get_width()//2), (rc.camera.get_height(), rc.camera.get_width()))
if info is not None: #checks if there are AR markers, then gets the contour of the outlining color
rc_utils.draw_ar_markers(ar_image, info, info_id, (0, 255, 0))
contours_ar_red = rc_utils.find_contours(ar_image, RED[0], RED[1])
contours_ar_blue = rc_utils.find_contours(ar_image, BLUE[0], BLUE[1])
contours_ar_green = rc_utils.find_contours(ar_image, GREEN[0],
GREEN[1])
red_largest = rc_utils.get_largest_contour(contours_ar_red, 2000)
blue_largest = rc_utils.get_largest_contour(contours_ar_blue, 2000)
green_largest = rc_utils.get_largest_contour(contours_ar_green, 2000)
if red_largest is not None:
redc = rc_utils.get_contour_center(red_largest)
red_dir = color_dir(redc, info, red_dir)
rc_utils.draw_contour(ar_image, red_largest)
if blue_largest is not None:
bluec = rc_utils.get_contour_center(blue_largest)
blue_dir = color_dir(bluec, info, blue_dir)
rc_utils.draw_contour(ar_image, blue_largest)
if green_largest is not None:
greenc = rc_utils.get_contour_center(green_largest)
green_dir = color_dir(greenc, info, green_dir)
rc_utils.draw_contour(ar_image, green_largest)
# if red_dir != 0 or green_dir != 0 or blue_dir != 0:
if red_dir == 0:
THIRD_PRI = red_dir
elif blue_dir == 0:
THIRD_PRI = blue_dir
else:
THIRD_PRI = green_dir
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
# TODO: Park the car 30 cm away from the closest orange cone.
# Use both color and depth information to handle cones of multiple sizes.
# You may wish to copy some of your code from lab2b.py
image = rc.camera.get_color_image()
contours = rc_utils.find_contours(image, ORANGE[0], ORANGE[1])
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
contour_center = rc_utils.get_contour_center(contour)
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
else:
contour_center = 0
scan = rc.lidar.get_samples()
__, coneDist = rc_utils.get_lidar_closest_point(scan, (-20, 20))
if contour is not None:
global pastTerm
global derivTerm
angleTerm = contour_center[1] - rc.camera.get_width() / 2
speedTerm = coneDist - 50
derivTerm = (speedTerm - pastTerm) / rc.get_delta_time(
) if speedTerm != pastTerm else derivTerm
speedSign = speedTerm / abs(speedTerm) if speedTerm != 0 else 0
print(str(speedTerm) + " and " + str(derivTerm))
angle = angleTerm * (1 / 200) #angle P controller
speed = speedTerm * (1 / 50) + derivTerm * (1 / 250
) #speed P"D" controller
angle = -1 if angle < -1 else angle
angle = 1 if angle > 1 else angle
speed = -1 if speed < -1 else speed
speed = 1 if speed > 1 else speed
#speed = 0 if abs(derivTerm) < 15 else speed
else:
speed = 0
angle = 0
pastTerm = speedTerm
rc.drive.set_speed_angle(speed, angle)
def run_phase(self, rc, depth_image, color_image, lidar_scan):
# print("FAST", self.fast_col, "SLOW", self.slow_col)
self.cropped_img = np.copy(color_image)[rc.camera.get_height() * 2 //
3:rc.camera.get_height(), :]
# SLOW CONTOUR INFO GATHERING
# Finds distance to largest slow contour >>
largest_slow = rc_utils.get_largest_contour(
rc_utils.find_contours(color_image, self.slow_col.value[0],
self.slow_col.value[1]),
c.LANE_MIN_CONTOUR_AREA)
if largest_slow is not None:
center_slow = rc_utils.get_contour_center(largest_slow)
dist_slow = rc_utils.get_pixel_average_distance(
depth_image, center_slow)
else:
dist_slow = 9999
# -------------------------------------- <<
if self.cur_state == self.State.FAST:
self.run_fast(rc, dist_slow)
# If the the slow contour is within a certain range, switch states
if dist_slow <= c.STATE_SWITCH_DIST:
self.cur_state = self.State.HARD_STOP
elif self.cur_state == self.State.SLOW or self.cur_state == self.State.HARD_STOP:
if self.cur_state == self.State.HARD_STOP:
self.stop_counter += 1
if self.stop_counter >= 10:
self.stop_counter = 0
self.cur_state = self.State.SLOW
# Runs function and gets output (# of slow contours visible)
out = self.run_slow(rc)
if out == 0:
self.cur_state = self.State.FAST
self.slow_state_angle = 0
# If slow line area sum is big enough, align to right side of fast lane:
# If no visible fast lane:
# ------ Full turn /or/ Consider way to turn on purple line (sharp turn)
# If only one line visible:
# ------ Save history of left side and right side contours and determine what side the single contour is on
"""rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
def update_contour():
"""
Finds contours in the current color image and uses them to update contour_center
and contour_area
"""
global contour_center
global contour_area
image = rc.camera.get_color_image()
if image is None:
contour_center = None
contour_area = 0
else:
# TODO (challenge 1): Search for multiple tape colors with a priority order DONE
# (currently we only search for blue)
# Crop the image to the floor directly in front of the car
image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
# Find all of the colored contours
for color in color_priority:
contours = rc_utils.find_contours(image, color[0], color[1])
if len(contours) > 0:
break
# Select the largest contour
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
contour_area = rc_utils.get_contour_area(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
else:
contour_center = None
contour_area = 0
# Display the image to the screen
rc.display.show_color_image(image)
def update_contour():
"""
Finds contours in the current color image and uses them to update contour_center
and contour_area
"""
global contour_center
global contour_area
image = rc.camera.get_color_image()
if image is None:
contour_center = None
contour_area = 0
else:
# Crop the image to the floor directly in front of the car
image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
# Search for each color in priority order
for color in COLOR_PRIORITY:
# Find all of the contours of the current color
contours = rc_utils.find_contours(image, color[0], color[1])
# Select the largest contour
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
contour_area = rc_utils.get_contour_area(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
break
# If no contours are found for any color, set center and area accordingly
else:
contour_center = None
contour_area = 0
# Display the image to the screen
rc.display.show_color_image(image)
def updateContour(color_priority):
image = rc.camera.get_color_image()
if image is None:
contour_center = None
contour_area = 0
else:
image_cropped = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
for color in color_priority:
contours = rc_utils.find_contours(image_cropped,color[0],color[1])
if len(contours) > 0:
break
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
contour_center = rc_utils.get_contour_center(contour)
contour_area = rc_utils.get_contour_area(contour)
else:
contour_center = None
contour_area = 0
rc.display.show_color_image(image)
return contour_center,contour_area
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
# Display the color image cropped to the top left
if rc.controller.was_pressed(rc.controller.Button.A):
image = rc.camera.get_color_image()
cropped = rc_utils.crop(
image, (0, 0),
(rc.camera.get_height() // 2, rc.camera.get_width() // 2))
rc.display.show_color_image(cropped)
# Find and display the largest red contour in the color image
if rc.controller.was_pressed(rc.controller.Button.B):
image = rc.camera.get_color_image()
contours = rc_utils.find_contours(image, RED[0], RED[1])
largest_contour = rc_utils.get_largest_contour(contours)
if largest_contour is not None:
center = rc_utils.get_contour_center(largest_contour)
area = rc_utils.get_contour_area(largest_contour)
print("Largest red contour: center={}, area={:.2f}".format(
center, area))
rc_utils.draw_contour(image, largest_contour,
rc_utils.ColorBGR.green.value)
rc_utils.draw_circle(image, center, rc_utils.ColorBGR.yellow.value)
rc.display.show_color_image(image)
else:
print("No red contours found")
# Print depth image statistics and show the cropped upper half
if rc.controller.was_pressed(rc.controller.Button.X):
depth_image = rc.camera.get_depth_image()
# Measure average distance at several points
left_distance = rc_utils.get_pixel_average_distance(
depth_image,
(rc.camera.get_height() // 2, rc.camera.get_width() // 4),
)
center_distance = rc_utils.get_depth_image_center_distance(depth_image)
center_distance_raw = rc_utils.get_depth_image_center_distance(
depth_image, 1)
right_distance = rc_utils.get_pixel_average_distance(
depth_image,
(rc.camera.get_height() // 2, 3 * rc.camera.get_width() // 4),
)
print(f"Depth image left distance: {left_distance:.2f} cm")
print(f"Depth image center distance: {center_distance:.2f} cm")
print(f"Depth image raw center distance: {center_distance_raw:.2f} cm")
print(f"Depth image right distance: {right_distance:.2f} cm")
# Measure pixels where the kernel falls off the edge of the photo
upper_left_distance = rc_utils.get_pixel_average_distance(
depth_image, (2, 1), 11)
lower_right_distance = rc_utils.get_pixel_average_distance(
depth_image,
(rc.camera.get_height() - 2, rc.camera.get_width() - 5), 13)
print(f"Depth image upper left distance: {upper_left_distance:.2f} cm")
print(
f"Depth image lower right distance: {lower_right_distance:.2f} cm")
# Find closest point in bottom third
cropped = rc_utils.crop(
depth_image,
(0, 0),
(rc.camera.get_height() * 2 // 3, rc.camera.get_width()),
)
closest_point = rc_utils.get_closest_pixel(cropped)
closest_distance = cropped[closest_point[0]][closest_point[1]]
print(
f"Depth image closest point (upper half): (row={closest_point[0]}, col={closest_point[1]}), distance={closest_distance:.2f} cm"
)
rc.display.show_depth_image(cropped, points=[closest_point])
# Print lidar statistics and show visualization with closest point highlighted
if rc.controller.was_pressed(rc.controller.Button.Y):
lidar = rc.lidar.get_samples()
front_distance = rc_utils.get_lidar_average_distance(lidar, 0)
right_distance = rc_utils.get_lidar_average_distance(lidar, 90)
back_distance = rc_utils.get_lidar_average_distance(lidar, 180)
left_distance = rc_utils.get_lidar_average_distance(lidar, 270)
print(f"Front LIDAR distance: {front_distance:.2f} cm")
print(f"Right LIDAR distance: {right_distance:.2f} cm")
print(f"Back LIDAR distance: {back_distance:.2f} cm")
print(f"Left LIDAR distance: {left_distance:.2f} cm")
closest_sample = rc_utils.get_lidar_closest_point(lidar)
print(
f"Closest LIDAR point: {closest_sample[0]:.2f} degrees, {closest_sample[1]:.2f} cm"
)
rc.display.show_lidar(lidar, highlighted_samples=[closest_sample])
# Print lidar distance in the direction the right joystick is pointed
rjoy_x, rjoy_y = rc.controller.get_joystick(rc.controller.Joystick.RIGHT)
if abs(rjoy_x) > 0 or abs(rjoy_y) > 0:
lidar = rc.lidar.get_samples()
angle = (math.atan2(rjoy_x, rjoy_y) * 180 / math.pi) % 360
distance = rc_utils.get_lidar_average_distance(lidar, angle)
print(f"LIDAR distance at angle {angle:.2f} = {distance:.2f} cm")
# Default drive-style controls
left_trigger = rc.controller.get_trigger(rc.controller.Trigger.LEFT)
right_trigger = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
left_joystick = rc.controller.get_joystick(rc.controller.Joystick.LEFT)
rc.drive.set_speed_angle(right_trigger - left_trigger, left_joystick[0])
def find_cones():
"""
Find the closest red and blue cones and update corresponding global variables.
"""
global red_center
global red_distance
global prev_red_distance
global blue_center
global blue_distance
global prev_blue_distance
prev_red_distance = red_distance
prev_blue_distance = blue_distance
color_image = rc.camera.get_color_image()
depth_image = rc.camera.get_depth_image()
if color_image is None or depth_image is None:
red_center = None
red_distance = 0
blue_center = None
blue_distance = 0
print("No image found")
return
# Search for the red cone
contours = rc_utils.find_contours(color_image, RED[0], RED[1])
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
red_center = rc_utils.get_contour_center(contour)
red_distance = rc_utils.get_pixel_average_distance(depth_image, red_center)
# Only use count it if the cone is less than MAX_DISTANCE away
if red_distance <= MAX_DISTANCE:
rc_utils.draw_contour(color_image, contour, rc_utils.ColorBGR.green.value)
rc_utils.draw_circle(color_image, red_center, rc_utils.ColorBGR.green.value)
else:
red_center = None
red_distance = 0
else:
red_center = None
red_distance = 0
# Search for the blue cone
contours = rc_utils.find_contours(color_image, BLUE[0], BLUE[1])
contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA)
if contour is not None:
blue_center = rc_utils.get_contour_center(contour)
blue_distance = rc_utils.get_pixel_average_distance(depth_image, blue_center)
# Only use count it if the cone is less than MAX_DISTANCE away
if blue_distance <= MAX_DISTANCE:
rc_utils.draw_contour(color_image, contour, rc_utils.ColorBGR.yellow.value)
rc_utils.draw_circle(
color_image, blue_center, rc_utils.ColorBGR.yellow.value
)
else:
blue_center = None
blue_distance = 0
else:
blue_center = None
blue_distance = 0
rc.display.show_color_image(color_image)
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global speed
global angle
global cur_state
global PRIORITY
global prevangle
global cones_done
global cur_mode
global counter
# Get all images
image = rc.camera.get_color_image()
#cur_state == State.cone_slaloming
corners, ids = rc_utils.get_ar_markers(image)
length = len(corners)
if length > 0:
id = 300
index = 0
for idx in range(0, len(ids)):
if ids[idx] < id:
id = ids[idx]
index = idx
TL = corners[index][0][0]
TR = corners[index][0][1]
BL = corners[index][0][3]
area = (abs(TL[0] - TR[0]) +
abs(TL[1] - TR[1])) * (abs(TL[0] - BL[0]) + abs(TL[1] - BL[1]))
print(id[0], area)
if id[0] == 32 and area > 1900:
if cur_state is not State.cone_slaloming:
cur_mode = Mode.no_cones
counter = 0
cur_state = State.cone_slaloming
print("State: ", cur_state)
elif id[0] == 236 and area > 850:
cur_state = State.wall_parking
print("State: ", cur_state)
depth_image = rc.camera.get_depth_image()
###### Line Following State ######
if cur_state == State.line_following:
if image is None:
contour_center = None
else:
# Crop the image to the floor directly in front of the car
image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1])
colorContours = []
contour = None
colorContours = []
red = checkRed(image)
green = checkGreen(image)
#blue = checkBlue(image)
yellow = checkYellow(image)
for priority in PRIORITY:
if priority == "Y" and yellow is not None:
colorContours.append(yellow)
print("yellow")
elif priority == "R" and red is not None:
colorContours.append(red)
print("red")
elif priority == "G" and green is not None:
colorContours.append(green)
print("green")
if not colorContours:
angle = prevangle
contour = None
else:
contour = colorContours[0]
if contour is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(contour)
# Draw contour onto the image
rc_utils.draw_contour(image, contour)
rc_utils.draw_circle(image, contour_center)
#change
else:
contour_center = None
if contour_center is not None:
angle = rc_utils.remap_range(contour_center[1], 0,
rc.camera.get_width(), -1, 1,
True)
angle = rc_utils.clamp(angle, -1, 1)
prevangle = angle
# Display the image to the screen
rc.display.show_color_image(image)
##### Cone Slaloming State ######
elif cur_state == State.cone_slaloming:
print("cone slaloming")
update_cones()
###### Wall Parking State ######
elif cur_state == State.wall_parking:
print("Wall Parking")
# Get distance at 1/4, 2/4, and 3/4 width
center_dist = rc_utils.get_depth_image_center_distance(depth_image)
left_dist = rc_utils.get_pixel_average_distance(
depth_image, LEFT_POINT, KERNEL_SIZE)
right_dist = rc_utils.get_pixel_average_distance(
depth_image, RIGHT_POINT, KERNEL_SIZE)
print("distance", center_dist)
# Get difference between left and right distances
dist_dif = left_dist - right_dist
print("dist_dif", dist_dif)
# Remap angle
angle = rc_utils.remap_range(dist_dif, -MAX_DIST_DIF, MAX_DIST_DIF, -1,
1, True)
if abs(dist_dif) > 1:
print("entered")
angle = rc_utils.remap_range(dist_dif, -MAX_DIST_DIF, MAX_DIST_DIF,
-1, 1, True)
if center_dist > 20:
speed = 0.5
elif center_dist < 21 and center_dist > 10:
speed = rc_utils.remap_range(center_dist, 20, 10, 0.5, 0)
speed = rc_utils.clamp(speed, 0, 0.5)
else:
speed = 0
print("speed", speed)
rc.drive.set_speed_angle(speed, angle)
else:
# stop moving
rc.drive.stop()
print("angle", angle)
print("speed", speed)
rc.drive.set_speed_angle(0.6, angle)
def flane(color_lane):
global contour_center
global contour_area
global speed
global angle
global lane
global arcount
global a
global time
time += rc.get_delta_time()
cimage = rc.camera.get_color_image()
if cimage is None:
contour_center = None
contour_area = 0
else:
#splits image in two so that two separate contours followed
left = rc_utils.crop(
cimage, (360, 0),
(rc.camera.get_height(), rc.camera.get_width() // 2))
right = rc_utils.crop(cimage, (360, rc.camera.get_width() // 2),
(rc.camera.get_height(), rc.camera.get_width()))
both = [left, right]
contour_centers = []
contour_areas = []
#find largest contours of left
contours_pur = rc_utils.find_contours(left, color_lane[0],
color_lane[1])
print(contours_pur)
rc.display.show_color_image(left)
print(color_lane)
contour_left = rc_utils.get_largest_contour(contours_pur,
MIN_CONTOUR_AREA)
#if there are contours, finds center and adds to contour_centers list
if contour_left is not None:
left_center = rc_utils.get_contour_center(contour_left)
contour_centers.append(left_center)
rc_utils.draw_contour(left, contour_left, (255, 0, 0))
rc.display.show_color_image(left)
else:
contour_centers.append(None)
#find largest contours of right
contours = rc_utils.find_contours(right, color_lane[0], color_lane[1])
contour_right = rc_utils.get_largest_contour(contours,
MIN_CONTOUR_AREA)
#if there are contours, finds center and adds to contour_centers list
if contour_right is not None:
right_center = rc_utils.get_contour_center(contour_right)
contour_centers.append(right_center)
#rc_utils.draw_contour(right, contour_right)
#rc.display.show_color_image(right)
else:
contour_centers.append(None)
#adjusts car based on being in the center of two contours
if None not in contour_centers:
lane = True
image = rc.camera.get_color_image()
contour_distance = (
contour_centers[1][1] +
rc.camera.get_width() // 2) - contour_centers[0][1]
contour_center = (contour_centers[0][0] + 10,
(contour_distance // 2) + contour_centers[0][1])
else:
contour_center = None
speed = 1
angle = 0
if lane == True: #catches if goes off lane, else, adjusts based off of center of contours
if contour_centers[0] == None:
print("turn left")
angle = -1
elif contour_centers[1] == None:
print("turn right")
angle = 1
else:
angle = rc_utils.remap_range(contour_center[1], 0,
rc.camera.get_width(), -1, 1,
True)
speed = 1
##getting ar direction
if arcount == 0 and time > 2:
a = rc.camera.get_color_image()
a = rc_utils.crop(
a, (0, rc.camera.get_width() // 4),
(rc.camera.get_height(), rc.camera.get_width() -
(rc.camera.get_width() // 4)))
corners, ids = rc_utils.get_ar_markers(a)
rc_utils.draw_ar_markers(a, corners, ids)
if len(corners) > 0:
#print("getting dir")
if rc_utils.get_ar_direction(corners[0]) == Direction.LEFT:
print(rc_utils.get_ar_direction(corners[0]))
angle = -0.7
if rc_utils.get_ar_direction(corners[0]):
print(rc_utils.get_ar_direction(corners[0]))
angle = 0.7
speed = 1
rc.drive.set_speed_angle(0, 0)
def update_contour():
"""
Finds contours in the current color image and uses them to update contour_center
and contour_area
"""
global contour_center
global contour_area
global cur_state
global FIRST_PRI1
global SECOND_PRI1
#global THIRD_PRI
global red_dir
global blue_dir
global green_dir
contour_image = rc.camera.get_color_image()
if contour_image is None:
contour_center = None
contour_area = 0
else:
# TODO (challenge 1): Search for multiple tape colors with a priority order
# (currently we only search for blue)
# Crop the image to the floor directly in front of the car
contour_image = rc_utils.crop(contour_image, CROP_FLOOR[0],
CROP_FLOOR[1])
#Find all of the red contours
contours_red = rc_utils.find_contours(contour_image, RED[0], RED[1])
# Find all of the blue contours
contours_blue = rc_utils.find_contours(contour_image, BLUE[0], BLUE[1])
#Find all of the green contours
contours_green = rc_utils.find_contours(contour_image, GREEN[0],
GREEN[1])
# Select the largest contour
L_contour_blue = rc_utils.get_largest_contour(contours_blue,
MIN_CONTOUR_AREA)
L_contour_red = rc_utils.get_largest_contour(contours_red,
MIN_CONTOUR_AREA)
L_contour_green = rc_utils.get_largest_contour(contours_green,
MIN_CONTOUR_AREA)
# Priorities#####################################################################
if FIRST_PRI1:
if FIRST_PRI1 == red_dir:
FIRST_PRI = L_contour_red
if SECOND_PRI1 == blue_dir:
SECOND_PRI = L_contour_blue
THIRD_PRI = L_contour_green
else:
SECOND_PRI = L_contour_green
THIRD_PRI = L_contour_blue
elif FIRST_PRI1 == blue_dir:
FIRST_PRI = L_contour_blue
if SECOND_PRI1 == green_dir:
SECOND_PRI = L_contour_green
THIRD_PRI = L_contour_red
else:
SECOND_PRI = L_contour_red
THIRD_PRI = L_contour_green
elif FIRST_PRI1 == green_dir:
FIRST_PRI = L_contour_green
if SECOND_PRI1 == blue_dir:
SECOND_PRI = L_contour_blue
THIRD_PRI = L_contour_red
else:
SECOND_PRI = L_contour_red
THIRD_PRI = L_contour_blue
if FIRST_PRI is not None: # and contour_center_first<200:
# Calculate contour information
contour_center = rc_utils.get_contour_center(FIRST_PRI)
contour_area = rc_utils.get_contour_area(FIRST_PRI)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, FIRST_PRI, (0, 255, 0))
rc_utils.draw_circle(contour_image, contour_center)
elif SECOND_PRI is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(SECOND_PRI)
contour_area = rc_utils.get_contour_area(SECOND_PRI)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, SECOND_PRI, (0, 0, 255))
rc_utils.draw_circle(contour_image, contour_center)
elif THIRD_PRI is not None:
# Calculate contour information
contour_center = rc_utils.get_contour_center(THIRD_PRI)
contour_area = rc_utils.get_contour_area(THIRD_PRI)
# Draw contour onto the image
rc_utils.draw_contour(contour_image, THIRD_PRI, (255, 0, 0))
rc_utils.draw_circle(contour_image, contour_center)
else:
contour_center = None
contour_area = 0