

class Pid:
    def __init__(self, dt):
        self.dt = dt
        self.params = None

        # 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 set_params(self, params):
        self.params = params

    def reset(self):
        self.yi = 0
        self.d = 0
        self.awu = 0

    def process(self, err, d, scale=1.0):
        dt = self.dt
        kp = self.params['kp'] * scale
        ki = self.params['ki'] * scale
        kd = self.params['kd'] * scale
        kt = self.params['kt'] * scale

        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
