57 lines
1.1 KiB
Python
57 lines
1.1 KiB
Python
|
|
|
|
class Pid:
|
|
def __init__(self, params):
|
|
self.params = params
|
|
self.k = 1.0
|
|
|
|
# Integrator
|
|
self.yi = 0
|
|
|
|
# Differentiator
|
|
self.d = 0
|
|
|
|
# Anti windup
|
|
self.y_min = -1.0
|
|
self.y_max = 1.0
|
|
self.awu = 0
|
|
|
|
# Output
|
|
self.y = 0
|
|
|
|
def scale(self, k):
|
|
self.k = k
|
|
|
|
@staticmethod
|
|
def params(kp, ki, kd, kt):
|
|
p = dict(kp=kp, ki=ki, kd=kd, kt=kt)
|
|
return p
|
|
|
|
def reset(self):
|
|
self.yi = 0
|
|
self.d = 0
|
|
self.awu = 0
|
|
|
|
def process(self, err, d):
|
|
dt = self.params['dt']
|
|
kp = self.params['kp'] * self.k
|
|
ki = self.params['ki'] * self.k
|
|
kd = self.params['kd'] * self.k
|
|
kt = self.params['kt'] * self.k
|
|
|
|
self.yi = self.yi + ki*dt * err + kt*dt * self.awu
|
|
yd = kd/dt*(d - self.d)
|
|
yp = kp * err
|
|
|
|
self.d = d
|
|
|
|
y = yp + self.yi + yd
|
|
|
|
self.y = max(self.y_min, min(self.y_max, y))
|
|
|
|
# Anti windup
|
|
self.awu = self.y - y
|
|
|
|
def get_y(self):
|
|
return self.y
|