pid.py
1.42 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
# -*- coding: utf-8 -*-
"""
author Benoit Dubois
brief PID servo
licence GPL v3+
"""
DEFAULT_PID_OMAX = 32000
DEFAULT_PID_INT_MAX = DEFAULT_PID_OMAX * 16
DEFAULT_PID_OSCALE = 1
class PIDfixed(object):
def __init__(self, kp=1, ki=0, sp=0, osp=0, imax=DEFAULT_PID_INT_MAX,
omax=DEFAULT_PID_OMAX, oscale=DEFAULT_PID_OSCALE):
"""
kp, ki: proportional and integral parameters
sp: setpoint
osp: output setpoint (output bias)
imax: integrator maximum value
omax: output range is [-omax, +omax]
oscale: output scaling, output is devided by 2**oscale
"""
self.kp = kp
self.ki = ki
self.sp = sp
self.osp = osp
self.imax = imax
self.omax = omax
self.oscale = oscale
self.un = 0
self._int_en = True
self._sum = 0
def compute(self, y0):
eps = self.sp - y0
if self._int_en is True:
sum_ = min(self.imax, max(-self.imax, self._sum + eps))
else:
sum_ = 0
vn = (self.kp * eps + self.ki * sum_) >> self.oscale
vn += self.osp
self.un = min(self.omax, max(-self.omax, vn))
self._sum = sum_ + self.un - vn # Anti windup
return self.un
@property
def integrator(self):
return self._sum
def enable_int(self, value=True):
self._int_en = value
def reset(self):
self._sum = 0