return path
def follow_path(path):
current_coord = path[0]
remaining_path = path[1:]
for new_coord in remaining_path:
move_to_coord(current_coord, new_coord)
current_coord = new_coord
# CHƯƠNG TRÌNH CHÍNH
# ==================
dt.set_drive_velocity(60) # percent
dt.set_turn_velocity(50) # percent
gyro.start_calibration() # cam bien phuong huong
gyro.set_heading(0)
MAZE = [
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[2, 1, 1, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 1, 0, 1, 1, 0],
[0, 0, 1, 0, 0, 1, 0, 1, 0, 0],
[0, 0, 1, 1, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 1, 0, 0, 0, 1, 1, 0],
[0, 0, 0, 1, 0, 1, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 1, 1, 1, 0, 0],
[0, 0, 0, 0, 0, 3, 0, 0, 0, 0],
# tỉ lệ bánh răng 1:1
drivetrain = Drivetrain(
motor_left, motor_right,
200, 200, DistanceUnits.MM, 1)
# khởi tạo đèn LED cảm ứng
touch_led = Touchled(Ports.PORT10)
# khởi tạo bumper trước
bumper_front = Bumper(Ports.PORT3)
# ĐỊNH NGHĨA HẰNG SỐ, THAM SỐ
# ===========================
robot_started = False
drivetrain.set_drive_velocity(100)
# ĐỊNH NGHĨA CÁC HÀM
# ==================
def flash_led(color, duration_in_seconds):
touch_led.on_hue(color)
sys.sleep(duration_in_seconds)
def dance():
for _ in range(2):
flash_led(ColorHue.YELLOW, 0.3)
flash_led(ColorHue.RED, 0.3)
motor_right = Motor(Ports.PORT1, True) # Reverse Polarity
motor_left = Motor(Ports.PORT2)
# khởi tạo drivetrain (bộ truyền động) với 2 motor
# mỗi motor gắn một bánh xe chu vi 200mm
# và khoảng cách giữa 2 bánh xe 2 bên là 202mm
# tỉ lệ bánh răng 1:1
drivetrain = Drivetrain(motor_left, motor_right, 200, 202, DistanceUnits.MM)
# khởi tạo đèn LED cảm ứng
touch_led = Touchled(Ports.PORT10)
# ĐỊNH NGHĨA HẰNG SỐ, THAM SỐ
# ===========================
robot_started = False
drivetrain.set_drive_velocity(100) # dinh nghia toc do mac dinh cua robot
# ĐỊNH NGHĨA CÁC HÀM
# ==================
def flash_led(color, duration_in_seconds):
touch_led.on_hue(color)
sys.sleep(duration_in_seconds)
def dance():
drivetrain.turn_for(LEFT, 30) # re trai (LEFT) 90 do
drivetrain.turn_for(RIGHT, 30) # re phai (RIGHT) 90 do
for _ in range(2): # lap 2 lan
flash_led(ColorHue.YELLOW, 0.3) # goi ham flash_led o phia tren
flash_led(ColorHue.RED, 0.3) # goi ham flash_led o phia tren
