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)])