Untitled
unknown
plain_text
a year ago
1.7 kB
4
Indexable
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())
Editor is loading...
Leave a Comment