64 lines
1.8 KiB
Lua
64 lines
1.8 KiB
Lua
--------------------------------------------------------------------
|
|
-- lib/pid.lua : regulateur PID concu pour le zero-depassement.
|
|
-- - derivee sur la MESURE (pas de coup au changement de consigne)
|
|
-- et FILTREE (EMA) contre le bruit des capteurs
|
|
-- - integrale conditionnelle: gelee quand la sortie sature dans
|
|
-- le sens de l'erreur (anti-emballement)
|
|
--------------------------------------------------------------------
|
|
local Pid = {}
|
|
Pid.__index = Pid
|
|
|
|
-- gains { kp, ki, kd } lus PAR REFERENCE a chaque pas (edition vive)
|
|
function Pid.nouveau(gains, borneMin, borneMax, iMax, filtreD)
|
|
return setmetatable({
|
|
gains = gains,
|
|
borneMin = borneMin,
|
|
borneMax = borneMax,
|
|
iMax = iMax or 40,
|
|
filtreD = filtreD or 0.5, -- 1 = brut, 0.5 = lisse
|
|
integrale = 0,
|
|
mesurePrec = nil,
|
|
deriveeF = 0,
|
|
}, Pid)
|
|
end
|
|
|
|
function Pid:raz()
|
|
self.integrale = 0
|
|
self.mesurePrec = nil
|
|
self.deriveeF = 0
|
|
end
|
|
|
|
function Pid:reglerGains(gains)
|
|
self.gains = gains
|
|
end
|
|
|
|
function Pid:calculer(consigne, mesure, dt)
|
|
local g = self.gains
|
|
local erreur = consigne - mesure
|
|
|
|
if self.mesurePrec and dt > 0 then
|
|
local brute = (mesure - self.mesurePrec) / dt
|
|
self.deriveeF = self.deriveeF
|
|
+ self.filtreD * (brute - self.deriveeF)
|
|
end
|
|
self.mesurePrec = mesure
|
|
|
|
local integrale = self.integrale + erreur * dt
|
|
if integrale > self.iMax then integrale = self.iMax end
|
|
if integrale < -self.iMax then integrale = -self.iMax end
|
|
|
|
local sortie = g.kp * erreur + g.ki * integrale
|
|
- g.kd * self.deriveeF
|
|
|
|
if sortie > self.borneMax then
|
|
if erreur < 0 then self.integrale = integrale end
|
|
return self.borneMax
|
|
elseif sortie < self.borneMin then
|
|
if erreur > 0 then self.integrale = integrale end
|
|
return self.borneMin
|
|
end
|
|
self.integrale = integrale
|
|
return sortie
|
|
end
|
|
|
|
return Pid
|