PIDController.py

import time

class PIDController:
    def __init__(self, kp, ki, kd, min_pwm, max_pwm):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.min_pwm = min_pwm
        self.max_pwm = max_pwm

        self.previous_error = 0
        self.integral = 0
        self.previous_time = time.time()

    def calculate_desired_airspeed(self, target_width, target_height):
        # Calculate the desired airspeed based on the target dimensions.
        # You can modify this function to use any other method of calculating
        # the desired airspeed based on the target_width and target_height.
        return (target_width * target_height) ** 0.5

    def update(self, airspeed, pwm, target_width, target_height):
        desired_airspeed = self.calculate_desired_airspeed(target_width, target_height)
        error = desired_airspeed - airspeed

        current_time = time.time()
        delta_time = current_time - self.previous_time

        self.integral += error * delta_time
        derivative = (error - self.previous_error) / delta_time

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

        new_pwm = pwm + output
        new_pwm = max(min(new_pwm, self.max_pwm), self.min_pwm)

        self.previous_error = error
        self.previous_time = current_time

        return new_pwm