Untitled

from hub import light_matrix, port
import motor, color_sensor
import time
import runloop



# Initialize the hub


# Initialize motors and color sensor
left_motor = motor.run(port.C, 5)
right_motor = Motor(Port., Direction.COUNTERCLOCKWISE)
color_sensor = ColorSensor(Port.E)

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 follow_line(color_sensor, left_motor, right_motor, pid):
    # Get the reflection from the color sensor (0 for black, 10 for white)
    reflection = color_sensor.reflection()
    threshold = 5# Midpoint between black and white
    error = reflection - threshold
    correction = pid.calculate(error)
    base_speed = 300# Base speed of the motors
    left_motor.run(base_speed - correction)
    right_motor.run(base_speed + correction)

# Initialize PID controller with tuned parameters
pid = PID(0.6, 0.03, 0.2)

# Main control loop
runloop.run(main())
while True:
    follow_line(color_sensor, left_motor, right_motor, pid)
    wait(10)# Wait for 10 milliseconds before the next loop iteration


async def main():
    # write your code here
    await light_matrix.write("Hi!")

runloop.run(main())