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 run_slow(self, rc):
largest = self.get_2_largest(self.cropped_img, self.slow_col)
if self.cur_state == self.State.SLOW:
speed = c.SPEED_SLOW
else:
speed = -1
# If the fast lane is visible more than the slow one, then just keep turning in the previously set direction
largest_fast_cropped = rc_utils.get_largest_contour(
rc_utils.find_contours(self.cropped_img, self.fast_col.value[0],
self.fast_col.value[1]))
largest_slow_cropped = rc_utils.get_largest_contour(
rc_utils.find_contours(self.cropped_img, self.slow_col.value[0],
self.slow_col.value[1]))
slow_a = rc_utils.get_contour_area(
largest_slow_cropped) if largest_slow_cropped is not None else 0
fast_a = rc_utils.get_contour_area(
largest_fast_cropped) if largest_fast_cropped is not None else 0
if len(largest) == 2 and not slow_a < fast_a:
self.angle = 0
elif len(largest) == 1 and not slow_a < fast_a:
cont = largest[0][1]
# Finds the top point of the contour and the bottom (Estimates line slope)
top_pt = tuple([
pt[0] for pt in cont if pt[0][1] == np.amin(cont[:, :, 1])
][0])
bott_pt = tuple([
pt[0] for pt in cont if pt[0][1] == np.amax(cont[:, :, 1])
][0])
# Slop is sloppy?????????????
if self.slow_state_angle == 0:
if top_pt[0] - bott_pt[0] > 0:
self.slow_state_angle = 1
elif top_pt[0] - bott_pt[0] < 0:
self.slow_state_angle = -1
else:
self.slow_state_angle = 0
self.angle = self.slow_state_angle
# Draws VIZHUALS
#cv.line(self.cropped_img, top_pt, bott_pt, (255, 255, 0), thickness=2)
#rc.display.show_color_image(self.cropped_img)
# Sets speed and angle
rc.drive.set_speed_angle(rc_utils.clamp(speed, -1, 1),
rc_utils.clamp(self.angle, -1, 1))
return len(largest)
def run_manual(self):
# Gets speed using triggers
rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
lt = rc.controller.get_trigger(rc.controller.Trigger.LEFT)
speed = rt - lt
# Gets angle from x axis of left joystick
angle = rc.controller.get_joystick(rc.controller.Joystick.LEFT)[0]
# Sets speed and angle
rc.drive.set_speed_angle(rc_utils.clamp(speed, -1, 1), rc_utils.clamp(angle, -1, 1))
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global speed
global angle
global cur_state
global weird
# Search for contours in the current color image
# Choose an angle based on contour_center#########################################
# If we could not find a contour, keep the previous angle (Bang Bang)
print(cur_state)
weird = ar_in_range()
if cur_state == State.lineFollow:
update_contour()
KEEP = 0
if weird == 2 or weird == 1:
print("switching state")
cur_state = State.laneFollow
if contour_center is not None:
#implement contour area center further in direction in proportion to necessary angle
# TODO (warmup): Implement a smoother way to follow the line
angle = rc_utils.remap_range(contour_center[1], 0,
rc.camera.get_width(), -1, 1)
KEEP = rc_utils.remap_range(abs(angle), 0, 1, 0, 200)
if angle > 0:
angle = rc_utils.remap_range(contour_center[1] + KEEP, 0,
rc.camera.get_width(), -1, 1)
angle = rc_utils.clamp(angle, -1, 1)
elif angle < 0:
angle = rc_utils.remap_range(contour_center[1] - KEEP, 0,
rc.camera.get_width(), -1, 1)
angle = rc_utils.clamp(angle, -1, 1)
speed = rc_utils.remap_range(abs(angle), 0, 1, .7, 1)
if cur_state == State.laneFollow:
if weird == 2:
flane(PURPLE)
else:
flane(ORANGE)
rc.drive.set_speed_angle(speed, angle)
def followLine(color_priority):
cCenter,cArea = updateContour(color_priority)
if cCenter is not None:
errorTerm = cCenter[1]-320
angle = errorTerm/320
else:
angle = 0
return rc_utils.clamp(angle*2,-1,1)
def run_phase(self, rc, depth_image, color_image, lidar_scan):
global prev_angle
self.updateContour(rc, depth_image, color_image)
"""
if contourcenter is not none, finds angle based on contourcenter location,
then adjusts the goal amount to turn based on if the angle is positive or negative.
Goal amount will increase the sharpness of the turn based on how sharp it already
is to ensure sharp turns are followed decently.
"""
# Notes: I think that I've tweaked the car to work consistently. Still need to
# make a reverse function and a finding function. Also a speed function.
if self.contour_center:
#max_line = 0.86
angle1 = rc_utils.remap_range(self.contour_center[1], 0,
rc.camera.get_width(), -1, 1)
goal_amount = rc_utils.remap_range(abs(angle1), 0, 1, 0, 70)
prev_angle = angle1
if angle1 > 0:
angle1 = rc_utils.remap_range(
self.contour_center[1] - goal_amount, 0,
rc.camera.get_width(), -1, 1)
angle = rc_utils.clamp(angle1, -1, 1)
prev_angle = angle
elif angle1 < 0:
angle1 = rc_utils.remap_range(
self.contour_center[1] + goal_amount, 0,
rc.camera.get_width(), -1, 1)
angle = rc_utils.clamp(angle1, -1, 1)
prev_angle = angle
else:
if angle1 == 0:
angle = 0
if angle1 is None:
angle = prev_angle
speed = rc_utils.remap_range(
abs(angle1), 0, 1, 1,
0.01) #desmos speed angle functions angle plug in for x
else: #change this to a finding function or reverse function
speed = 1
angle = 0
rc.drive.set_speed_angle(speed, angle)
def run_approach_state(self, rc):
# Calculates distance change to see if cone was passed
if self.__contour_distance is not None:
if self.__prev_distance is None:
self.__prev_distance = self.__contour_distance
distance_change = self.__contour_distance - self.__prev_distance
self.__consec_no_cont = 0
else:
distance_change = None
self.__consec_no_cont += 1
dist_opp = self.get_closest_opposite_contour()
# If distance makes a huge jump, assume that it is now following a different cone so switch states
if self.__consec_no_cont >= self.Const.MAX_CONSEC_CONTOURS.value or (distance_change > self.Const.MAX_DIST_DIFF.value and dist_opp < self.__contour_distance):
self.__cur_col = self.Color.BLUE if self.__cur_col == self.Color.RED else self.Color.RED
self.__cur_state = self.State.PASS
else:
# >>>> Proportional control
# -------------------------
# Updates speed variable
self.update_speed()
# Scales the center offset boundaries based on screen width
w = rc.camera.get_width()
low_set = self.Const.LOW_SET_PROPORTION.value * (w / 2)
high_set = self.Const.HIGH_SET_PROPORTION.value * (w / 2)
# Calculates new setpoint based on how close the car is to the cone.
if self.__contour_distance is not None:
self.__approach_setpoint = rc_utils.remap_range(self.__contour_distance, self.Const.APPROACH_DIST_UPPER.value,
self.Const.APPROACH_DIST_LOWER.value, low_set, high_set)
# negates setpoint if color is red. This ensures that the car passes cone on correct side
self.__approach_setpoint *= (-1 if self.__cur_col == self.Color.RED else 1)
"""DEBUG: Draw Setpoint"""
#rc_utils.draw_circle(self.__color_image, (rc.camera.get_height() // 2, int((w / 2) + setpoint)))
# Gets angle from setpoint using proportional control
kP = 3
if self.__contour_distance is not None:
self.__approach_angle = rc_utils.clamp(rc_utils.remap_range(self.__contour_center[1], self.__approach_setpoint,
rc.camera.get_width() + self.__approach_setpoint, -1, 1) * kP, -1, 1)
self.__approach_speed = rc_utils.remap_range(abs(self.__approach_angle), 1, 0, 0.1, 1)
#self.__approach_speed = 1
# Sets speed angle
rc.drive.set_speed_angle(.5*self.__approach_speed, self.__approach_angle)
"""DEBUG: Show image"""
def start():
"""
This function is run once every time the start button is pressed
"""
global max_speed
global show_triggers
global show_joysticks
print("Start function called")
rc.set_update_slow_time(0.5)
rc.drive.stop()
max_speed = 0.25
show_triggers = False
show_joysticks = False
# Test numeric functions
assert rc_utils.remap_range(5, 0, 10, 0, 50) == 25
assert rc_utils.remap_range(5, 0, 20, 1000, 900) == 975
assert rc_utils.remap_range(2, 0, 1, -10, 10) == 30
assert rc_utils.remap_range(2, 0, 1, -10, 10, True) == 10
assert rc_utils.clamp(3, 0, 10) == 3
assert rc_utils.clamp(-2, 0, 10) == 0
assert rc_utils.clamp(11, 0, 10) == 10
# Print start message
print(
">> Test Utils: A testing program for the racecar_utils library.\n"
"\n"
"Controls:\n"
" Right trigger = accelerate forward\n"
" Left trigger = accelerate backward\n"
" Left joystick = turn front wheels\n"
" Right joystick = measure LIDAR distance in a specific direction when clicked\n"
" A button = Take a color image and crop it to the top left\n"
" B button = Take a color image and identify the largest red contour\n"
" X button = Take a depth image and print several statistics\n"
" Y button = Take a lidar scan and print several statistics\n"
" Right bumper = Take a color image and identify the AR markers\n"
" Left bumper = Throw an exception\n")
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
scan = rc.lidar.get_samples()
highlighted_samples = [
] # Samples we will highlight in the LIDAR visualization
# Choose speed based on forward distance
_, front_dist = rc_utils.get_lidar_closest_point(
scan, (-FRONT_DIST_ANGLE, FRONT_DIST_ANGLE))
speed = rc_utils.remap_range(front_dist, 0, BRAKE_DISTANCE, 0, MAX_SPEED,
True)
# Measure distance to the left and right walls ahead of the car
left_ahead = rc_utils.get_lidar_average_distance(scan, -SIDE_ANGLE)
right_ahead = rc_utils.get_lidar_average_distance(scan, SIDE_ANGLE)
ratio = left_ahead / right_ahead if left_ahead > 0 and right_ahead > 0 else 1.0
# If there is a wall ahead and the left wall is significantly father, assume that
# the hallway has turned left
if front_dist < BRAKE_DISTANCE and ratio > LEFT_TURN_RATIO:
angle = -1
print("HARD LEFT: ratio", ratio, "front dist", front_dist)
# Otherwise, try to stay GOAL_DIST away from the right wall
else:
right_wall_angle, right_wall_dist = rc_utils.get_lidar_closest_point(
scan, WINDOW_ANGLES)
angle = rc_utils.remap_range(right_wall_dist, GOAL_DIST / 2, GOAL_DIST,
-1, 0)
angle = rc_utils.clamp(angle, -1, 1)
# Display the closest point on the right wall
highlighted_samples = [(right_wall_angle, right_wall_dist)]
rc.drive.set_speed_angle(speed, angle)
rc.display.show_lidar(scan, highlighted_samples=highlighted_samples)
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()
# If no cone is found, stop
if contour_center is None or contour_area == 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_AREA
if cur_mode == Mode.park:
speed = rc_utils.remap_range(contour_area, GOAL_AREA / 2,
GOAL_AREA, 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 contour_area < FORWARD_AREA else Mode.reverse
# FORWARD MODE: Move forward until area is greater than REVERSE_AREA
elif cur_mode == Mode.forward:
speed = rc_utils.remap_range(contour_area, MIN_CONTOUR_AREA,
REVERSE_AREA, 1.0, 0.0)
speed = rc_utils.clamp(speed, 0, ALIGN_SPEED)
# Once we pass REVERSE_AREA, switch to reverse mode
if contour_area > REVERSE_AREA:
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 area is less than FORWARD_AREA
else:
speed = rc_utils.remap_range(contour_area, REVERSE_AREA,
FORWARD_AREA, -1.0, 0.0)
speed = rc_utils.clamp(speed, -ALIGN_SPEED, 0)
# Once we pass FORWARD_AREA, switch to forward mode
if contour_area < FORWARD_AREA:
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 area 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, "Area:", contour_area)
# 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
"""
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
"""
global cur_speed
global prev_forward_dist
global prev_back_dist
# 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 in front of and behind the car
scan = rc.lidar.get_samples()
_, forward_dist = rc_utils.get_lidar_closest_point(scan, FRONT_WINDOW)
_, back_dist = rc_utils.get_lidar_closest_point(scan, REAR_WINDOW)
frame_speed: float
if forward_dist < back_dist:
frame_speed = (prev_forward_dist - forward_dist) / rc.get_delta_time()
else:
frame_speed = -(prev_back_dist - back_dist) / rc.get_delta_time()
cur_speed += ALPHA * (frame_speed - cur_speed)
prev_forward_dist = forward_dist
prev_back_dist = back_dist
stop_distance: float
# FORARD SAFETY STOP: We are about to hit something in front of us
if not rc.controller.is_down(rc.controller.Button.RB) and cur_speed > 0:
# Calculate slow and stop distances based on the current 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 we are past slow_distance, reduce speed proportional to how close we are
# to stop_distance
if stop_distance < forward_dist < slow_distance:
speed = min(
speed,
rc_utils.remap_range(forward_dist, stop_distance,
slow_distance, 0, 0.5),
)
print("Forward 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 < forward_dist < stop_distance:
speed = rc_utils.remap_range(forward_dist, 0, stop_distance, -4,
-0.2)
speed = rc_utils.clamp(speed, -1, -0.2)
print("Forward safety stop: reversing at {}".format(speed))
# REAR SAFETY STOP: We are about to hit something behind us
if not rc.controller.is_down(rc.controller.Button.LB) and cur_speed < 0:
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 stop_distance < back_dist < slow_distance:
speed = max(
speed,
rc_utils.remap_range(back_dist, stop_distance, slow_distance,
0, -0.5),
)
print("Back safety slow: speed limited to {}".format(speed))
if 0 < back_dist < stop_distance:
speed = rc_utils.remap_range(back_dist, 0, stop_distance, 4, 0.2,
True)
speed = rc_utils.clamp(speed, 0.2, 1)
print("Back safety stop: accelerating forward 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 distance of the closest object in front of and behind the car
if rc.controller.is_down(rc.controller.Button.B):
print("Forward distance:", forward_dist, "Back distance:", back_dist)
# 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 LIDAR scan
rc.display.show_lidar(scan)
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
global cur_mode
global counter
find_cones()
angle: float
speed: float
# Align ourselves to smoothly approach and pass the red cone while it is in view
if cur_mode == Mode.red_align:
# Once the red cone is out of view, enter Mode.red_pass
if (
red_center is None
or red_distance == 0
or red_distance - prev_red_distance > CLOSE_DISTANCE
):
if 0 < prev_red_distance < FAR_DISTANCE:
counter = max(SHORT_PASS_TIME, counter)
cur_mode = Mode.red_pass
else:
cur_mode = Mode.no_cones
# If it seems like we are not going to make the turn, enter Mode.red_reverse
elif (
red_distance < REVERSE_DISTANCE
and red_center[1] > rc.camera.get_width() // 4
):
counter = REVERSE_BRAKE_TIME
cur_mode = Mode.red_reverse
# Align with the cone so that it gets closer to the left side of the screen
# as we get closer to it, and slow down as we approach
else:
goal_point = rc_utils.remap_range(
red_distance,
CLOSE_DISTANCE,
FAR_DISTANCE,
0,
rc.camera.get_width() // 4,
True,
)
angle = rc_utils.remap_range(
red_center[1], goal_point, rc.camera.get_width() // 2, 0, 1
)
angle = rc_utils.clamp(angle, -1, 1)
speed = rc_utils.remap_range(
red_distance,
CLOSE_DISTANCE,
FAR_DISTANCE,
MIN_ALIGN_SPEED,
MAX_ALIGN_SPEED,
True,
)
elif cur_mode == Mode.blue_align:
if (
blue_center is None
or blue_distance == 0
or blue_distance - prev_blue_distance > CLOSE_DISTANCE
):
if 0 < prev_blue_distance < FAR_DISTANCE:
counter = max(SHORT_PASS_TIME, counter)
cur_mode = Mode.blue_pass
else:
cur_mode = Mode.no_cones
elif (
blue_distance < REVERSE_DISTANCE
and blue_center[1] < rc.camera.get_width() * 3 // 4
):
counter = REVERSE_BRAKE_TIME
cur_mode = Mode.blue_reverse
else:
goal_point = rc_utils.remap_range(
blue_distance,
CLOSE_DISTANCE,
FAR_DISTANCE,
rc.camera.get_width(),
rc.camera.get_width() * 3 // 4,
True,
)
angle = rc_utils.remap_range(
blue_center[1], goal_point, rc.camera.get_width() // 2, 0, -1
)
angle = rc_utils.clamp(angle, -1, 1)
speed = rc_utils.remap_range(
blue_distance,
CLOSE_DISTANCE,
FAR_DISTANCE,
MIN_ALIGN_SPEED,
MAX_ALIGN_SPEED,
True,
)
# Curve around the cone at a fixed speed for a fixed time to pass it
if cur_mode == Mode.red_pass:
angle = rc_utils.remap_range(counter, 1, 0, 0, -0.5)
speed = PASS_SPEED
counter -= rc.get_delta_time()
# After the counter expires, enter Mode.blue_align if we see the blue cone,
# and Mode.blue_find if we do not
if counter <= 0:
cur_mode = Mode.blue_align if blue_distance > 0 else Mode.blue_find
elif cur_mode == Mode.blue_pass:
angle = rc_utils.remap_range(counter, 1, 0, 0, 0.5)
speed = PASS_SPEED
counter -= rc.get_delta_time()
if counter <= 0:
cur_mode = Mode.red_align if red_distance > 0 else Mode.red_find
# If we know we are supposed to be aligning with a red cone but do not see one,
# turn to the right until we find it
elif cur_mode == Mode.red_find:
angle = 1
speed = FIND_SPEED
if red_distance > 0:
cur_mode = Mode.red_align
elif cur_mode == Mode.blue_find:
angle = -1
speed = FIND_SPEED
if blue_distance > 0:
cur_mode = Mode.blue_align
# If we are not going to make the turn, reverse while keeping the cone in view
elif cur_mode == Mode.red_reverse:
if counter >= 0:
counter -= rc.get_delta_time()
speed = -1
angle = 1
else:
angle = -1
speed = REVERSE_SPEED
if (
red_distance > STOP_REVERSE_DISTANCE
or red_center[1] < rc.camera.get_width() // 10
):
counter = LONG_PASS_TIME
cur_mode = Mode.red_align
elif cur_mode == Mode.blue_reverse:
if counter >= 0:
counter -= rc.get_delta_time()
speed = -1
angle = 1
else:
angle = 1
speed = REVERSE_SPEED
if (
blue_distance > STOP_REVERSE_DISTANCE
or blue_center[1] > rc.camera.get_width() * 9 / 10
):
counter = LONG_PASS_TIME
cur_mode = Mode.blue_align
# If no cones are seen, drive forward until we see either a red or blue cone
elif cur_mode == Mode.no_cones:
angle = 0
speed = NO_CONES_SPEED
if red_distance > 0 and blue_distance == 0:
cur_mode = Mode.red_align
elif blue_distance > 0 and red_distance == 0:
cur_mode = Mode.blue_align
elif blue_distance > 0 and red_distance > 0:
cur_mode = (
Mode.red_align if red_distance < blue_distance else Mode.blue_align
)
rc.drive.set_speed_angle(speed, angle)
print(
f"Mode: {cur_mode.name}, red_distance: {red_distance:.2f} cm, blue_distance: {blue_distance:.2f} cm, speed: {speed:.2f}, angle: {angle:2f}"
)
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 run_phase(self, rc, depth_image, color_image, lidar_scan):
#print(">> Running Wall Following")
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
# Use the triggers to control the car's speed left joystick for angle
#rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
#lt = rc.controller.get_trigger(rc.controller.Trigger.LEFT)
#speed = rt - lt
#angle = rc.controller.get_joystick(rc.controller.Joystick.LEFT)[0]
# Calculate the distance
scan = lidar_scan
max_wall = 0.65 #testing max speed
hall = rc.camera.get_width() // 9
optimum = 60
rightDist = rc_utils.get_lidar_average_distance(scan, 44, 10)
leftDist = rc_utils.get_lidar_average_distance(scan, 316, 10)
angle = rc_utils.remap_range(rightDist - leftDist, -hall, hall, -1, 1)
angle = rc_utils.clamp(angle, -1, 1)
# get them tags
corners, ids = rc_utils.get_ar_markers(color_image)
billy = 150
if c.ar_in_range_ID(billy, depth_image, color_image, rc,
4 / 5) == c.ID_DIRECTION:
dirrrrrrrrr = rc_utils.get_ar_direction(corners[0])
#print(dirrrrrrrrr)
if dirrrrrrrrr == rc_utils.Direction.LEFT:
angle = -1
elif dirrrrrrrrr == rc_utils.Direction.RIGHT:
angle = 1
self.ar_tag = True
elif self.is_canyon:
tooClose = 80
if rc_utils.get_depth_image_center_distance(
depth_image) < tooClose:
angle = 1
right_farthest = np.amax(
depth_image[rc.camera.get_height() * 5 // 6,
rc.camera.get_width() //
2:rc.camera.get_width()].flatten())
left_farthest = np.amax(
depth_image[rc.camera.get_height() * 5 // 6,
0:rc.camera.get_width() // 2].flatten())
diff = abs(right_farthest - left_farthest)
AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW = 100
if self.ar_tag and self.ledge_count == 0 and diff > 50:
if right_farthest > AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW:
self.many += 1
self.ledge_angle = -1
self.ledge_count = 10
elif left_farthest > AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW:
self.many += 1
self.ledge_angle = 1
self.ledge_count = 10
#print("left ", left_farthest, " right ", right_farthest)
speed = rc_utils.remap_range(abs(angle), 15, 1, 1, 0.5) #temp controls
if self.many == 3:
self.ar_tag = False
if self.ledge_count > 0:
angle = self.ledge_angle
self.ledge_count -= 1
rc.drive.set_speed_angle(max_wall * speed, 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)])