def get_closest_contour(contours, depth_img):
MIN_CONTOUR_AREA = 70
MAX_CONTOUR_DIST = 300
# Gets centers of each contour and extracts countours based on conditions
centers = []
for idx, contour in enumerate(contours):
cent = rc_utils.get_contour_center(contour)
if cent is not None:
dist = rc_utils.get_pixel_average_distance(depth_img, cent)
area = rc_utils.get_contour_area(contour)
if area > MIN_CONTOUR_AREA and dist < MAX_CONTOUR_DIST:
centers.append((idx, rc_utils.get_contour_center(contour)))
indexes = [center[0] for center in centers]
centers = [center[1] for center in centers]
# Calculates the distance to each center
distances = [rc_utils.get_pixel_average_distance(depth_img, (center[0], center[1])) for center in centers]
conts = [contours[index] for index in indexes]
# Finds smallest distance and index of that distance
if len(conts):
minimum = min(enumerate(distances), key=lambda x: x[1])
# (index, min dist)
# Returns distance to closest contour center, the contour itself, and the center position
return (minimum[1], conts[minimum[0]], centers[minimum[0]])
else:
# If there is no contour, my love for humanities is returned
return None
def ar_in_range_color(RANGE, d_img, c_img, colors):
#gets depth and ar tags, if there are some, finds center and then compares depth with
# RANGE. If within range, prints ids
depth_image = d_img
ar_image = c_img
ar_image = rc_utils.crop(
ar_image, (0, 0), (rc.camera.get_height() // 2, rc.camera.get_width()))
checking_info, _ = rc_utils.get_ar_markers(ar_image)
if checking_info:
x = (int)((checking_info[0][0][0][1] + checking_info[0][0][1][1]) // 2)
y = (int)((checking_info[0][0][0][0] + checking_info[0][0][1][0]) // 2)
if rc_utils.get_pixel_average_distance(depth_image, (x, y)) < RANGE:
contours = [
rc_utils.find_contours(ar_image, color.value[0],
color.value[1]) for color in colors
]
largest_contours = [(idx, rc_utils.get_largest_contour(cont, 2000))
for idx, cont in enumerate(contours)]
if len(largest_contours):
return colors[max(
largest_contours,
key=lambda x: get_cont_area_proofed(x[1]))[0]]
def ar_in_range():
depth_image = rc.camera.get_depth_image()
ar_image = rc.camera.get_color_image()
ar_image = rc_utils.crop(
ar_image, (0, 0), (rc.camera.get_height() // 2, rc.camera.get_width()))
checking_info, checking_info_id = rc_utils.get_ar_markers(ar_image)
if checking_info:
x = (int)((checking_info[0][0][0][1] + checking_info[0][0][1][1]) // 2)
y = (int)((checking_info[0][0][0][0] + checking_info[0][0][1][0]) // 2)
if rc_utils.get_pixel_average_distance(depth_image, (x, y)) < 200:
contours_ar_orange = rc_utils.find_contours(
ar_image, ORANGE[0], ORANGE[1])
contours_ar_purp = rc_utils.find_contours(ar_image, PURPLE[0],
PURPLE[1])
orange_largest = rc_utils.get_largest_contour(
contours_ar_orange, 2000)
purp_largest = rc_utils.get_largest_contour(contours_ar_purp, 2000)
if orange_largest is not None:
print("orange")
return 1
elif purp_largest is not None:
print("purple")
return 2
else:
return 0
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 ar_in_range_ID(RANGE, d_img, c_img):
#gets depth and ar tags, if there are some, finds center and then compares depth with
# RANGE. If within range, prints ids
depth_image = d_img
ar_image = c_img
ar_image = rc_utils.crop(
ar_image, (0, 0), (rc.camera.get_height() // 2, rc.camera.get_width()))
checking_info, checking_info_id = rc_utils.get_ar_markers(ar_image)
if checking_info:
x = (int)((checking_info[0][0][0][1] + checking_info[0][0][1][1]) // 2)
y = (int)((checking_info[0][0][0][0] + checking_info[0][0][1][0]) // 2)
if rc_utils.get_pixel_average_distance(depth_image, (x, y)) < RANGE:
return (checking_info_id)
return None
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global cur_mode
# Measure distance at the left, right, and center of the image
depth_image = rc.camera.get_depth_image()
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
)
# Use the difference between left_dist and right_dist to determine angle
dist_dif = left_dist - right_dist
angle = rc_utils.remap_range(dist_dif, -MAX_DIST_DIF, MAX_DIST_DIF, -1, 1, True)
# PARK MODE: More forward or backward until center_dist is GOAL_DIST
if cur_mode == Mode.park:
speed = rc_utils.remap_range(center_dist, GOAL_DIST * 2, GOAL_DIST, 1.0, 0.0)
speed = rc_utils.clamp(speed, -PARK_SPEED, PARK_SPEED)
# If speed is close to 0, round to 0 to "park" the car
if -SPEED_THRESHOLD < speed < SPEED_THRESHOLD:
speed = 0
# If the angle is no longer correct, choose mode based on area
if abs(angle) > ANGLE_THRESHOLD:
cur_mode = Mode.forward if center_dist > FORWARD_DIST else Mode.reverse
# FORWARD MODE: Move forward until we are closer that REVERSE_DIST
elif cur_mode == Mode.forward:
speed = rc_utils.remap_range(center_dist, FORWARD_DIST, REVERSE_DIST, 1.0, 0.0)
speed = rc_utils.clamp(speed, 0, ALIGN_SPEED)
# Once we pass REVERSE_DIST, switch to reverse mode
if center_dist < REVERSE_DIST:
cur_mode = Mode.reverse
# If we are close to the correct angle, switch to park mode
if abs(angle) < ANGLE_THRESHOLD:
cur_mode = Mode.park
# REVERSE MODE: move backward until we are farther than FORWARD_DIST
else:
speed = rc_utils.remap_range(center_dist, REVERSE_DIST, FORWARD_DIST, -1.0, 0.0)
speed = rc_utils.clamp(speed, -ALIGN_SPEED, 0)
# Once we pass FORWARD_DIST, switch to forward mode
if center_dist > FORWARD_DIST:
cur_mode = Mode.forward
# If we are close to the correct angle, switch to park mode
if abs(angle) < ANGLE_THRESHOLD:
cur_mode = Mode.park
# Reverse the angle if we are driving backward
if speed < 0:
angle *= -1
rc.drive.set_speed_angle(speed, angle)
# Display the depth image, and show LEFT_POINT and RIGHT_POINT
rc.display.show_depth_image(depth_image, points=[LEFT_POINT, RIGHT_POINT])
# Print the current speed and angle when the A button is held down
if rc.controller.is_down(rc.controller.Button.A):
print("Speed:", speed, "Angle:", angle)
# Print measured distances when the B button is held down
if rc.controller.is_down(rc.controller.Button.B):
print(
"left_dist:",
left_dist,
"center_dist:",
center_dist,
"right_dist:",
right_dist,
)
# Print the current mode when the X button is held down
if rc.controller.is_down(rc.controller.Button.X):
print("Mode:", cur_mode)
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 update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global cur_speed
global prev_distance
# Use the triggers to control the car's speed
rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
lt = rc.controller.get_trigger(rc.controller.Trigger.LEFT)
speed = rt - lt
# Calculate the distance of the object directly in front of the car by cropping
# out a window directly in front of the car and finding the closest point
depth_image = rc.camera.get_depth_image()
depth_image_cropped = rc_utils.crop(depth_image, (0, LEFT_COL),
(BOTTOM_ROW, RIGHT_COL))
closest_point = rc_utils.get_closest_pixel(depth_image_cropped)
distance = rc_utils.get_pixel_average_distance(depth_image_cropped,
closest_point)
# Update forward speed estimate
frame_speed = (prev_distance - distance) / rc.get_delta_time()
cur_speed += ALPHA * (frame_speed - cur_speed)
prev_distance = distance
# Calculate slow and stop distances based on the forward speed
stop_distance = rc_utils.clamp(
MIN_STOP_DISTANCE + cur_speed * abs(cur_speed) * STOP_DISTANCE_SCALE,
MIN_STOP_DISTANCE,
MAX_STOP_DISTANCE,
)
slow_distance = stop_distance * SLOW_DISTANCE_RATIO
if not rc.controller.is_down(rc.controller.Button.RB) and cur_speed > 0:
# If we are past slow_distance, reduce speed proportional to how close we are
# to stop_distance
if stop_distance < distance < slow_distance:
speed = min(
speed,
rc_utils.remap_range(distance, stop_distance, slow_distance, 0,
0.5),
)
print("Safety slow: speed limited to {}".format(speed))
# Safety stop if we are passed stop_distance by reversing at a speed
# proportional to how far we are past stop_distance
if 0 < distance < stop_distance:
speed = rc_utils.remap_range(distance, 0, stop_distance, -4, -0.2,
True)
speed = rc_utils.clamp(speed, -1, -0.2)
print("Safety stop: reversing at {}".format(speed))
# Use the left joystick to control the angle of the front wheels
angle = rc.controller.get_joystick(rc.controller.Joystick.LEFT)[0]
rc.drive.set_speed_angle(speed, angle)
# Print the current speed and angle when the A button is held down
if rc.controller.is_down(rc.controller.Button.A):
print("Speed:", speed, "Angle:", angle)
# Print the depth image closest distance when the B button is held down
if rc.controller.is_down(rc.controller.Button.B):
print("Distance:", distance)
# Print cur_speed estimate and stop distance when the X button is held down
if rc.controller.is_down(rc.controller.Button.X):
print("Current speed estimate: {:.2f} cm/s, Stop distance: {:.2f}".
format(cur_speed, stop_distance))
# Display the current depth image
rc.display.show_depth_image(depth_image,
points=[(closest_point[0],
closest_point[1] + LEFT_COL)])
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global speed
global angle
global cur_mode
# Search for contours in the current color image
update_contour()
# Find the distance of the cone contour
if contour_center is not None:
depth_image = rc.camera.get_depth_image()
distance = rc_utils.get_pixel_average_distance(depth_image, contour_center)
# If no cone is found, stop
if contour_center is None or distance == 0.0:
speed = 0
angle = 0
else:
# Use proportional control to set wheel angle based on contour x position
angle = rc_utils.remap_range(contour_center[1], 0, rc.camera.get_width(), -1, 1)
# PARK MODE: Move forward or backward until contour_area is GOAL_DISTANCE
if cur_mode == Mode.park:
speed = rc_utils.remap_range(
distance, GOAL_DISTANCE * 2, GOAL_DISTANCE, 1.0, 0.0
)
speed = rc_utils.clamp(speed, -PARK_SPEED, PARK_SPEED)
# If speed is close to 0, round to 0 to "park" the car
if -SPEED_THRESHOLD < speed < SPEED_THRESHOLD:
speed = 0
# If the angle is no longer correct, choose mode based on area
if abs(angle) > ANGLE_THRESHOLD:
cur_mode = Mode.forward if distance > FORWARD_DISTANCE else Mode.reverse
# FORWARD MODE: Move forward until we are closer that REVERSE_DISTANCE
elif cur_mode == Mode.forward:
speed = rc_utils.remap_range(
distance, FORWARD_DISTANCE, REVERSE_DISTANCE, 1.0, 0.0
)
speed = rc_utils.clamp(speed, 0, ALIGN_SPEED)
# Once we pass REVERSE_DISTANCE, switch to reverse mode
if distance < REVERSE_DISTANCE:
cur_mode = Mode.reverse
# If we are close to the correct angle, switch to park mode
if abs(angle) < ANGLE_THRESHOLD:
cur_mode = Mode.park
# REVERSE MODE: move backward until we are farther than FORWARD_DISTANCE
else:
speed = rc_utils.remap_range(
distance, REVERSE_DISTANCE, FORWARD_DISTANCE, -1.0, 0.0
)
speed = rc_utils.clamp(speed, -ALIGN_SPEED, 0)
# Once we pass FORWARD_DISTANCE, switch to forward mode
if distance > FORWARD_DISTANCE:
cur_mode = Mode.forward
# If we are close to the correct angle, switch to park mode
if abs(angle) < ANGLE_THRESHOLD:
cur_mode = Mode.park
# Reverse the angle if we are driving backward
if speed < 0:
angle *= -1
rc.drive.set_speed_angle(speed, angle)
# Print the current speed and angle when the A button is held down
if rc.controller.is_down(rc.controller.Button.A):
print("Speed:", speed, "Angle:", angle)
# Print the center and distance of the largest contour when B is held down
if rc.controller.is_down(rc.controller.Button.B):
if contour_center is None:
print("No contour found")
else:
print("Center:", contour_center, "Distance:", distance)
# Print the current mode when the X button is held down
if rc.controller.is_down(rc.controller.Button.X):
print("Mode:", cur_mode)
