From c7e0a717b6cfe6e150547753b07658d20ce66697 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=8E=8B=E5=BC=80=E6=99=BA?= Date: Tue, 9 Jul 2019 16:25:57 +0800 Subject: [PATCH] Create pid.py --- scripts/libraries/pid.py | 46 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 scripts/libraries/pid.py diff --git a/scripts/libraries/pid.py b/scripts/libraries/pid.py new file mode 100644 index 000000000..b4bba1a90 --- /dev/null +++ b/scripts/libraries/pid.py @@ -0,0 +1,46 @@ +from pyb import millis +from math import pi, isnan + +class PID: + _kp = _ki = _kd = _integrator = _imax = 0 + _last_error = _last_derivative = _last_t = 0 + _RC = 1/(2 * pi * 20) + def __init__(self, p=0, i=0, d=0, imax=0): + self._kp = float(p) + self._ki = float(i) + self._kd = float(d) + self._imax = abs(imax) + self._last_derivative = float('nan') + + def get_pid(self, error, scaler): + tnow = millis() + dt = tnow - self._last_t + output = 0 + if self._last_t == 0 or dt > 1000: + dt = 0 + self.reset_I() + self._last_t = tnow + delta_time = float(dt) / float(1000) + output += error * self._kp + if abs(self._kd) > 0 and dt > 0: + if isnan(self._last_derivative): + derivative = 0 + self._last_derivative = 0 + else: + derivative = (error - self._last_error) / delta_time + derivative = self._last_derivative + \ + ((delta_time / (self._RC + delta_time)) * \ + (derivative - self._last_derivative)) + self._last_error = error + self._last_derivative = derivative + output += self._kd * derivative + output *= scaler + if abs(self._ki) > 0 and dt > 0: + self._integrator += (error * self._ki) * scaler * delta_time + if self._integrator < -self._imax: self._integrator = -self._imax + elif self._integrator > self._imax: self._integrator = self._imax + output += self._integrator + return output + def reset_I(self): + self._integrator = 0 + self._last_derivative = float('nan')