jens
08d8c32106
- many other improvements git-svn-id: http://moon:8086/svn/projects/HendiControl@116 fda53097-d464-4ada-af97-ba876c37ca34
59 lines
1.1 KiB
Python
59 lines
1.1 KiB
Python
import numpy as np;
|
|
|
|
|
|
class Pid():
|
|
def __init__(self):
|
|
# Integrator
|
|
self.yi = 0
|
|
|
|
# Differentiator
|
|
self.xd = 0
|
|
|
|
# Auto windup
|
|
self.y_min = 0.0
|
|
self.y_max = 100.0
|
|
self.diff_aw = 0.0
|
|
|
|
# Output
|
|
self.y = 0
|
|
self.yc = 0
|
|
|
|
@staticmethod
|
|
def params(kp, ki, kd, rho):
|
|
p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
|
|
return p
|
|
|
|
def process(self, dt, params, err):
|
|
kp = params['kp']
|
|
ki = params['ki']
|
|
kd = params['kd']
|
|
rho = params['rho']
|
|
|
|
err_aw = err + self.diff_aw
|
|
yi = rho*self.yi + err_aw
|
|
yd = err - self.xd
|
|
|
|
_yp = kp * err
|
|
_yi = ki*dt * yi
|
|
_yd = kd/dt * yd
|
|
|
|
self.y = _yp + _yi + _yd
|
|
|
|
yaw = yi
|
|
if yaw < self.y_min:
|
|
self.diff_aw = abs(yaw - self.y_min)
|
|
|
|
if yaw > self.y_max:
|
|
self.diff_aw = -abs(yaw - self.y_max)
|
|
|
|
self.yc = max(self.y_min, min(self.y_max, self.y))
|
|
|
|
self.yi = yi
|
|
self.xd = err
|
|
|
|
def get_y(self):
|
|
return self.y
|
|
|
|
def get_yc(self):
|
|
return self.yc
|