50 lines
951 B
Python
50 lines
951 B
Python
import numpy as np
|
|
|
|
|
|
class Pid:
|
|
def __init__(self):
|
|
# Integrator
|
|
self.yi = 0
|
|
|
|
# Differentiator
|
|
self.err = 0
|
|
|
|
# Auto windup
|
|
self.y_min = -1.0
|
|
self.y_max = 1.0
|
|
|
|
# Output
|
|
self.y = 0
|
|
|
|
@staticmethod
|
|
def params(kp, ki, kd, rho):
|
|
p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
|
|
return p
|
|
|
|
def reset(self):
|
|
self.yi = 0
|
|
self.err = 0
|
|
|
|
def process(self, dt, params, err):
|
|
kp = params['kp']
|
|
ki = params['ki']
|
|
kd = params['kd']
|
|
rho = params['rho']
|
|
|
|
self.yi = rho*self.yi + ki*dt * err
|
|
yd = kd/dt*(err - self.err)
|
|
yp = kp * err
|
|
|
|
# Reset yi on error sign changes
|
|
if yp > self.y_max or yp <= self.y_min:
|
|
self.yi = 0
|
|
|
|
self.err = err
|
|
|
|
y = yp + self.yi + yd
|
|
|
|
self.y = max(self.y_min, min(self.y_max, y))
|
|
|
|
def get_y(self):
|
|
return self.y
|