feat: 初始化提交

pid.py

@@ -0,0 +1,33 @@
+class PID:
+    def __init__(self, kp, ki, kd, integral_limit=10.0, alpha=1.0, output_limit=(0.0, 1.0)):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.prev_error = 0.0
+        self.integral = 0.0
+        self.integral_limit = integral_limit
+        self.alpha = alpha
+        self.prev_output = 0.0
+        self.output_limit = output_limit
+
+    def update(self, error, dt):
+        if dt <= 0:
+            dt = 1e-6
+        self.integral += error * dt
+        self.integral = max(-self.integral_limit, min(self.integral_limit, self.integral))
+        derivative = (error - self.prev_error) / dt
+        output = self.kp * error + self.ki * self.integral + self.kd * derivative
+        self.prev_error = error
+        # 一阶滤波
+        output = self.alpha * output + (1 - self.alpha) * self.prev_output
+        self.prev_output = output
+
+        # 输出限制
+        output = max(self.output_limit[0], min(self.output_limit[1], output))
+
+        return output
+
+    def reset(self):
+        self.prev_error = 0.0
+        self.integral = 0.0
+        self.prev_output = 0.0