按轨迹运行（pid)

import math
import time

import numpy as np
import matplotlib.pyplot as plt

class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.last_error = 0
        self.integral = 0

    def compute_control(self, error):
        # Proportional term
        P = self.Kp * error

        # Integral term
        self.integral += error
        I = self.Ki * self.integral

        # Derivative term
        derivative = error - self.last_error
        D = self.Kd * derivative
        self.last_error = error

        # Compute control signal
        control = P + I + D

        return control



def plot_arrow(x, y, yaw, length=5, width=1):
    dx = length * math.cos(yaw)
    dy = length * math.sin(yaw)
    plt.arrow(x, y, dx, dy, head_len