61 lines
1.7 KiB
Lua
61 lines
1.7 KiB
Lua
--------------------------------------------------------------------
|
|
-- lib/pid.lua : regulateur PID concu pour le zero-depassement
|
|
-- - derivee calculee sur la MESURE (pas de coup de derivee au
|
|
-- changement de consigne)
|
|
-- - integrale conditionnelle: gelee quand la sortie sature dans le
|
|
-- sens de l'erreur (anti-emballement)
|
|
--------------------------------------------------------------------
|
|
local Pid = {}
|
|
Pid.__index = Pid
|
|
|
|
-- gains: { kp, ki, kd } ; bornes de sortie ; borne d'integrale
|
|
function Pid.nouveau(gains, borneMin, borneMax, iMax)
|
|
return setmetatable({
|
|
gains = gains,
|
|
borneMin = borneMin,
|
|
borneMax = borneMax,
|
|
iMax = iMax or 40,
|
|
integrale = 0,
|
|
mesurePrec = nil,
|
|
}, Pid)
|
|
end
|
|
|
|
function Pid:raz()
|
|
self.integrale = 0
|
|
self.mesurePrec = nil
|
|
end
|
|
|
|
function Pid:reglerGains(gains)
|
|
self.gains = gains
|
|
end
|
|
|
|
function Pid:calculer(consigne, mesure, dt)
|
|
local g = self.gains
|
|
local erreur = consigne - mesure
|
|
|
|
local derivee = 0
|
|
if self.mesurePrec and dt > 0 then
|
|
derivee = (mesure - self.mesurePrec) / dt
|
|
end
|
|
self.mesurePrec = mesure
|
|
|
|
-- tentative d'integration bornee
|
|
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 * derivee
|
|
|
|
if sortie > self.borneMax then
|
|
-- sature haut: on n'integre que si l'erreur tire vers le bas
|
|
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
|