Blame view
software/common_class/pid.py
1.42 KB
14b996da3
First commit
|
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
|