PID Python Demo

# pip install matplotlib
import time
import matplotlib.pyplot as plt
class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.prev_error = 0
        self.integral = 0
    def calculate(self, setpoint,
                current_value):
        error = setpoint - current_value
        self.integral += error
        derivative = error = self.prev_error
        # python cannot find self.kd,
        # but we defined it above.
        output = self.kp * error +\
            self.ki * self.integral +\
            self.kd * derivative
        self.prev_error = error
        return output

class AircraftSimulator:
    def __init__(self):
        self.height = 0
        self.velocity = 0
    def update(self, throttle, time_step):
        acceleration = throttle - 0.1 * self.velocity
        self.velocity += acceleration * time_step
        self.height += self.velocity * time_step

def main():
    kp = 5.0
    ki = 0.1
    kd = 10
    pid_controller = PIDController(kp, ki, id)
    aircraft_simulator = AircraftSimulator()
    target_height = 10.0
    # 步长0.1秒
    time_step = 0.1
    total_time = 20
    current_time = 0.0
    time_data = []
    height_data = []
    while (current_time < total_time):
        control_signal = pid_controller.calculate(
                target_height,
                aircraft_simulator.height)
        disturbance = -1.5
        control_signal += disturbance
        aircraft_simulator.update(
                control_signal,
                time_step)
        # 横坐标
        time_data.append(current_time)
        # 纵坐标
        height_data.append(
                aircraft_simulator.height)
        current_time += time_step
        time.sleep(time_step)
    print("Simulation completed.")

    plt.plot(time_data,
            height_data,
            label = 'Height')
    plt.axhline(
            y = target_height,
            color = 'r',
            linestyle = '--',
            label = 'Target Height')
    plt.xlabel('Time (s)')
    plt.ylabel('Height (m)')
    plt.title('Aircraft Height Control with Random Disturbance')
    plt.legend()
    plt.grid(True)
    plt.show()

if __name__ == '__main__':
    main()