Lego Mindstorm EV3 R2D2

main.py

#!/usr/bin/env pybricks-micropython

"""
Example LEGO® MINDSTORMS® EV3 Robot Educator Driving Base Program
-----------------------------------------------------------------

This program requires LEGO® EV3 MicroPython v2.0.
Download: https://education.lego.com/en-us/support/mindstorms-ev3/python-for-ev3

Building instructions can be found at:
https://education.lego.com/en-us/support/mindstorms-ev3/building-instructions#robot
"""

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import Motor, InfraredSensor
from pybricks.parameters import Port
from pybricks.robotics import DriveBase
import random
import math

from pybricks.media.ev3dev import Image, ImageFile, SoundFile

# Initialize the EV3 Brick.
ev3 = EV3Brick()
img = Image(ImageFile.DIZZY)
ev3.screen.load_image(img)

eyes = InfraredSensor(Port.S4)

degree = -3

# Initialize the motors.
head = Motor(Port.A)
left_motor = Motor(Port.B)
right_motor = Motor(Port.C)

base_head_angle = head.angle()

robot = DriveBase(left_motor, right_motor, wheel_diameter=30, axle_track=139)

d = 100
dead_end = False
while 1:
    try:
        # Go forward 0.3m
        if d > 5:
            robot.straight(-300)
        where_move = 0 # -1 left, 0 straihght, 1 right
        _max = 0
        for i in [0,-1,1]:
            d = eyes.distance()
            if d > _max and not (dead_end and i == 0):
                where_move = i
                _max = d
            dead_end = False
            
            if i == 0:
                head.run_target(150, -30*degree)
            if i == 1:
                head.run_target(150, 0)
            if i == -1:
                head.run_target(150, 30*degree)
        if _max <= 80:
            ev3.speaker.play_file(SoundFile.UH_OH)
            ev3.speaker.play_file(SoundFile.BACKWARDS)
            robot.straight(400)
            # rnd = int(round((random.random() - 0.5)/abs(random.random() - 0.5)))
            # robot.turn(90*rnd)
            dead_end = True
            d = 0
            continue

        if where_move == 0: # straight
            pass
        if where_move == 1: # right
            robot.turn(90)
        if where_move == -1: # left
            robot.turn(-90)

    except Exception as e:
        ev3.speaker.beep()
        head.run_target(150, 0)