# Controller Class
    # PID controller class
    # by Kai De La Cruz & Kenzie Goldman 2/17/25

    from pyb import Pin, Timer import time

    class PIDController: 
        '''A PID controller class for motor control''' 

        def__init__(self, kp, ki, kd, setpoint=0): 
        '''Initializes the PID controller with given gains and setpoint''' 
            self.kp = kp 
            self.ki = ki
            self.kd = kd 
            self.setpoint = setpoint 
            self.integral = 0
            self.previous_error = 0 
            self.previous_time = time.ticks_ms()

        def compute(self, measurement):
            '''Computes the control effort based on the measurement'''
            current_time = time.ticks_ms()
            dt = time.ticks_diff(current_time, self.previous_time) / 1000.0  
            error = self.setpoint - measurement
            self.integral += error * dt
            derivative = (error - self.previous_error) / dt if dt > 0 else 0

            output = self.kp * error + self.ki * self.integral + self.kd * derivative

            self.previous_error = error
            self.previous_time = current_time

            return output

        def set_setpoint(self, setpoint):
            '''Sets a new setpoint for the PID controller'''
            self.setpoint = setpoint
            self.integral = 0
            self.previous_error = 0
            self.previous_time = time.ticks_ms()