Slutt prosjekt kode

#!/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
from time import sleep

ev3 = EV3Brick()

# Reset --> Scan color --> Put color in Holdster --> Repeat til Button pressed OR maximum amount of colors has been added --> Drives and feeds colors to designated pots.

class ColorSorter():
    """Class for the Color Sorter robot."""
    def __init__(self):
        #Defines which ports the motors are connected to, and their corresponding name. 
        self.conveyor = Motor(Port.D)
        self.sorting_arm = Motor(Port.A)
        #Defines external input devices, including the color sensor and the touch button. 
        self.color_sensor = ColorSensor(Port.S3)
        self.touch_button = TouchSensor(Port.S1)
        #Defines speed variables for each of the motors. Big difference in their speed, as 
        #the arm requires actual momentum to move the ejector. 
        self.CONVEYOR_SPEED = 150
        self.ARM_SPEED = 1500

        #Creates a array/list of all possible and valid colors. 
        self.COLOR_VALUES = {
            Color.RED, Color.BLUE, 
            Color.YELLOW, Color.GREEN,
            Color.BROWN, Color.BLACK
            }
        # Stores color values with corresponding angles. 
        self.POSITIONS = {
            Color.RED: 50,
            Color.BLUE: 215,
            Color.GREEN: 400,
            Color.YELLOW: 570,
            Color.BROWN: 0, 
            Color.BLACK: 0, 
        }
        # Creates a empty array for storing the scanned colors. 
        self.COLOR_ARRAY = []
        
        # Resets the motor angles to 0, regardless of location. 
        self.conveyor.reset_angle(0)
        self.sorting_arm.reset_angle(0)
    
    def main(self):
        """Move sorting arm to home position (0 mm travelled)."""
        self.park()
        if self.touch_button.pressed():
            self.conveyor.stop()        
        # Runs a self test of the sorting arm, to ensure that it still works.  
        self.sorting_arm.run_angle(-self.ARM_SPEED, 70)
        self.sorting_arm.run_angle(self.ARM_SPEED, 70)
        sleep(5)

    def run(self):
        self.main()
        self.readColor()
        ev3.screen.print("Right button for manual.")
        ev3.screen.print("Left button for automated")
        while True: 
            if Button.RIGHT in ev3.buttons.pressed():
                self.manual_sorting()
                break
            elif Button.LEFT in ev3.buttons.pressed():
                self.automated_sorting()
                break
            wait(50)

    def manual_sorting(self):
        i = 0
        while i < len(self.COLOR_ARRAY):
            # Requires button. 
            if Button.RIGHT in ev3.buttons.pressed(): 
                color = self.COLOR_ARRAY[i]
                angle = self.POSITIONS[color]
                self.conveyor.run_angle(self.CONVEYOR_SPEED, angle)
                self.sorting_arm.run_angle(-self.ARM_SPEED, 70)
                self.sorting_arm.run_angle(self.ARM_SPEED, 70)
                ev3.screen.print(color)
                self.park()
                i += 1
                wait(500)  # Debounce delay to prevent multiple triggers

    def automated_sorting(self):
        i = 0 
        while i < len(self.COLOR_ARRAY):
            color = self.COLOR_ARRAY[i]
            angle = self.POSITIONS[color]
            self.conveyor.run_angle(self.CONVEYOR_SPEED, angle)
            self.sorting_arm.run_angle(-self.ARM_SPEED, 70)
            self.sorting_arm.run_angle(self.ARM_SPEED, 70)
            ev3.screen.print(color)
            self.park()
            i += 1
            wait(500)  # Debounce delay to prevent multiple triggers


    
    def park(self):
        """Stops when the position is reached."""
        # Returns the sorting arm to its "home" position. Also resets the angle counter. 
        while not self.touch_button.pressed():
            self.conveyor.run(-self.CONVEYOR_SPEED)
        self.conveyor.stop()
        self.conveyor.reset_angle(0)
        
    def readColor(self):
        # Runs the contents of the loop while the Color_array is smaller than 8 objects. 
        while len(self.COLOR_ARRAY) < 8:
            # Loads an image on the screen to indicate next steps. 
            ev3.screen.load_image(ImageFile.RIGHT)
            ev3.screen.print(len(self.COLOR_ARRAY))
            while True: 
                self.pressed_middle = Button.CENTER in ev3.buttons.pressed()
                self.color_collected = self.color_sensor.color()
                # Required this, as without it it would just add a color when it saw one, even if it was wrong. 
                if self.pressed_middle:
                    if self.color_collected in self.COLOR_VALUES:
                        break
                    break
            
            
            # Appends the color that was collected to the array. Notifies with sound, and text. 
            self.COLOR_ARRAY.append(self.color_collected)
            ev3.speaker.beep(100,100)
            ev3.screen.print(self.color_collected)
            # Doesnt let the program continue until the sensor no longer sees the same color temporarily. 
            while self.color_sensor.color() in self.COLOR_VALUES:
                pass
            sleep(.5)
            ev3.screen.clear()
                
colorsorter = ColorSorter()
colorsorter.run()