Untitled

#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
import time

#define the main ev3 brick used
ev3 = EV3Brick()

#define each of the things attached to the ev3 brick and their respective ports
leftMotor = Motor(Port.C)
rightMotor = Motor(Port.B)
colourSensor = ColorSensor(Port.S4)
distanceSensor = UltrasonicSensor(Port.S2)
button1 = TouchSensor(Port.S3)
button2 = TouchSensor(Port.S1)

#define the drivebase to control all the motors efficiently with their own functions (turn(), drive(), ect)
robot = DriveBase(leftMotor, rightMotor, 55.5, 162)

#define the variable used to determine if the robot has reached the end of the room
end_check = False

#press to start
while (button1.pressed() == False) and (button2.pressed() == False):
    pass

#define distance for ultrasonic sensor to trigger, and delay for the wait time between moving back and turning
distance = 100
delay = 100
#create a dictionary for the different if functions for each sensor
sensor_types = {
    "colour": 'colourSensor.color() != "None"',
    "distance": 'distanceSensor.distance() > distance',
    "bumper": 'button1.pressed() == False) and (button2.pressed() == False'
}
#define functions, then access them multiple times throughout the code with different variables being assigned to "type"
def sensor(Type):
    #define dictionary locally, 
    #because when the code looks for the dict, it first checks the function it is in, then the rest of the code, 
    #resulting in a more efficient response
    local_type = {
    "colour": 'colourSensor.color() != "None"',
    "distance": 'distanceSensor.distance() > distance',
    "bumper": 'button1.pressed() == False) and (button2.pressed() == False'
    }
    #if all need to be checked,
    if Type == "all":
        #check if each 
        return all(eval(v) for v in local_type.values())
    #if the type is not all, the if function will be passed and will return the result of the function (true or false)
    return eval(local_type[Type])

def drive():
    print("started driving")
    while sensor("all"):
        robot.drive(150, 0)
    #check each different sensor, by defining "types" as one of the keys in the directiory "sensor_types"
    #then run the sensor function, for the first sensor, if it returns as true, the for loop will be repeated, 
    #if a value is returned as false, the drive() function will end and the sensor type will be sent back to the while True: loop
    for types in sensor_types:
        if sensor(sensor_types[types]) == False:
            return sensor_types[types]
    #should always return at this point, stop the code if fails
    print("failed")
    exit()

def uhoh(Type, Angle, Position):
    #check if it needs to move back
    if (Type in "colour" or "bumper") or (Position == 3): 
        #moves back
        robot.straight(-50)
        wait(delay)
        if not sensor("all"):
            #if there's still something, move back
            uhoh(Type, Angle, 3)
            return
    #turn when there's nothing in the way
    robot.turn(Angle)
    wait(delay)
    #if already turned, start driving again
    if (Position == 2):
        return
    while sensor("all"):
        #save the starting time
        start_time = time.time()
        while ((time.time() - start_time) < 5):
            #drive for 5 seconds
            robot.drive(100, 0)
        #turn again
        uhoh(Type, Angle, 2)
        #reset end check variable when finish a full turn, then go back to driving
        end_check = False 
        return
    #if it reaches a wall when trying to turn twice in a row, switch start checking the other way
    if end_check == True:
        #reset variable
        end_check = False 
        print("reached end")
        #turn around
        robot.turn(2*Angle)
        return
    else:
        end_check == True
    # if didnt complete shorter distance
    for types in sensor_types:
        if sensor(sensor_types[types]) == False:
            uhoh(sensor_types[types], Angle, 2)
            return

#keep repeating the two functions over and over
while True:
    #drive until it hits a wall, then turn
    uhoh(drive(), 90, 1)
    #after reaching the next wall, turn the other way
    uhoh(drive(), -90, 1)