33 lines
1.1 KiB
Python
33 lines
1.1 KiB
Python
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 |