예제 #1
0
    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))
예제 #2
0
    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)
예제 #3
0
    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))
예제 #4
0
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)
예제 #5
0
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)
예제 #6
0
    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)
예제 #7
0
    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"""
예제 #8
0
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")
예제 #9
0
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)
예제 #10
0
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)
예제 #11
0
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)
예제 #12
0
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)
예제 #13
0
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}"
    )
예제 #14
0
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)
예제 #15
0
    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)
예제 #16
0
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)])