Untitled

from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Direction
from pybricks.tools import wait

class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.errors = []

    def sum_errors(self):
        return sum(self.errors)

    def P(self, error):
        return self.Kp * error

    def I(self, error):
        self.errors.append(error)
        return self.Ki * self.sum_errors()

    def D(self, error):
        if len(self.errors) >= 3:
            return self.Kd * ((self.errors[-1] - self.errors[-2]) + (self.errors[-2] - self.errors[-3])) / 2
        else:
            return 0

    def calculate(self, error):
        return self.P(error) + self.I(error) + self.D(error)

def is_red_detected(color_sensor):
    red_ratio_threshold = 1.5
    rgb = color_sensor.rgb()
    red, green, blue = rgb[0], rgb[1], rgb[2]
    red_intensity = red / (green + blue)
    return red_intensity > red_ratio_threshold

def is_blue_detected(color_sensor):
    blue_ratio_threshold = 1.5
    rgb = color_sensor.rgb()
    red, green, blue = rgb[0], rgb[1], rgb[2]
    blue_intensity = blue / (red + green)
    return blue_intensity > blue_ratio_threshold

def follow_line(color_sensor, left_motor, right_motor, pid):
    reflection = color_sensor.reflection()
    threshold = 45.25
    error = reflection - threshold
    correction = pid.calculate(error)
    left_motor.run(base_speed - correction)
    right_motor.run(base_speed + correction)

def calculate_base_speed(reflection):
    if reflection < 9.6:
        return 330
    elif reflection < 12.3:
        return 400
    elif reflection < 16.4:
        return 450
    elif reflection < 24.5:
        return 570
    elif reflection < 34.7:
        return 640
    elif reflection < 39.8:
        return 730
    elif reflection < 44.9:
        return 1000
    elif reflection < 50:
        return 1100
    elif reflection < 55:
        return 1000
    elif reflection < 60.1:
        return 750
    elif reflection < 65.2:
        return 640
    elif reflection < 70.3:
        return 570
    elif reflection < 75.4:
        return 540
    elif reflection < 80.5:
        return 460
    elif reflection < 85.6:
        return 400
    elif reflection < 90.7:
        return 330
    else:
        return 450

hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B, Direction.COUNTERCLOCKWISE)
color_sensor = ColorSensor(Port.E)

pid = PID(0.09172, 0.002290987654, 0.2377291432137)
base_speed = 500

while True:
    reflection = color_sensor.reflection()
    base_speed = calculate_base_speed(reflection)
    follow_line(color_sensor, left_motor, right_motor, pid)
    wait(10)