2つファイルがあります。下までスクロールしましょう！
"""
Copyright MIT and Harvey Mudd College
MIT License
Summer 2020
Lab 4A - LIDAR Safety Stop
"""
########################################################################################
# Imports
########################################################################################
import sys
import cv2 as cv
import numpy as np
sys.path.insert(0, "../../library")
import racecar_core
import racecar_utils as rc_utils
########################################################################################
# Global variables
########################################################################################
rc = racecar_core.create_racecar()
# >> Constants
# The (min, max) degrees to consider when measuring forward and rear distances
FRONT_WINDOW = (-10, 10)
REAR_WINDOW = (170, 190)
########################################################################################
# Functions
########################################################################################
def start():
"""
This function is run once every time the start button is pressed
"""
# Have the car begin at a stop
rc.drive.stop()
# Print start message
print(
">> Lab 4A - LIDAR Safety Stop\\n"
"\\n"
"Controls:\\n"
" Right trigger = accelerate forward\\n"
" Right bumper = override forward safety stop\\n"
" Left trigger = accelerate backward\\n"
" Left bumper = override rear safety stop\\n"
" Left joystick = turn front wheels\\n"
" A button = print current speed and angle\\n"
" B button = print forward and back distances"
)
def update():
"""
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
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)
# TODO (warmup): Prevent the car from hitting things in front or behind it.
# Allow the user to override safety stop by holding the left or right bumper.
if not (rc.controller.is_down(rc.controller.Button.LB) or rc.controller.is_down(rc.controller.Button.RB)):
if forward_dist <= 85.0:
speed = -1.0
elif back_dist <= 85.0:
speed = 1.0
# 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)
# Display the current LIDAR scan
rc.display.show_lidar(scan)
########################################################################################
# DO NOT MODIFY: Register start and update and begin execution
########################################################################################
if __name__ == "__main__":
rc.set_start_update(start, update, None)
rc.go()
"""
Copyright MIT and Harvey Mudd College
MIT License
Summer 2020
Lab 4B - LIDAR Wall Following
"""
########################################################################################
# Imports
########################################################################################
import sys
import cv2 as cv
import numpy as np
sys.path.insert(0, "../../library")
import racecar_core
import racecar_utils as rc_utils
########################################################################################
# Global variables
########################################################################################
rc = racecar_core.create_racecar()
# Add any global variables here
########################################################################################
# Functions
########################################################################################
def start():
"""
This function is run once every time the start button is pressed
"""
# Have the car begin at a stop
rc.drive.stop()
# Print start message
print(">> Lab 4B - LIDAR Wall Following")
def update():
"""
After start() is run, this function is run every frame until the back button
is pressed
"""
# TODO: Follow the wall to the right of the car without hitting anything.
scan = rc.lidar.get_samples()
speed = 0.0
angle = 0.0
LEFT = -1
RIGHT = 1
if rc.controller.is_down(rc.controller.Button.A):
_, right_wall_dist = rc_utils.get_lidar_closest_point(scan, (10, 75))
_, left_wall_dist = rc_utils.get_lidar_closest_point(scan, (-75, -10))
_, forward_wall_dist = rc_utils.get_lidar_closest_point(scan, (-45, 45))
left_front_dist = rc_utils.get_lidar_average_distance(scan, -38, 76)
right_front_dist = rc_utils.get_lidar_average_distance(scan, 38, 76)
speed = 0.5
if right_wall_dist < 30:
angle = LEFT
mode = 0
elif left_wall_dist < 30:
angle = RIGHT
mode = 1
elif right_front_dist > 70:
angle = RIGHT
mode = 2
elif left_front_dist > 70:
angle = LEFT
mode = 3
else:
speed = 0.2
angle = 0
mode = 4
if forward_wall_dist < 27:
angle = LEFT if angle == RIGHT else RIGHT
speed = -1
mode = 5
print(f"mode: {mode}")
rc.drive.set_speed_angle(speed, angle)
########################################################################################
# DO NOT MODIFY: Register start and update and begin execution
########################################################################################
if __name__ == "__main__":
rc.set_start_update(start, update, None)
rc.go()