45 lines
744 B
Matlab
45 lines
744 B
Matlab
function u = satFZ_PI(fis, e, Ke, Kde, Kdu, Ts)
|
|
%SATFZ_PI Calculate the output of the controller
|
|
% Must be called every Ts
|
|
%
|
|
% Output:
|
|
% u: Output
|
|
|
|
% Inputs:
|
|
% fis: FIS to use
|
|
% e: error
|
|
% Ke: error gain
|
|
% Kde: error derivative gain = a*Ke
|
|
% Kdu: du gain
|
|
% Ts: Sampling period
|
|
|
|
persistent prev_e;
|
|
persistent prev_u;
|
|
|
|
if isempty(prev_e) % first call
|
|
prev_e = 0;
|
|
prev_u = 0;
|
|
end
|
|
|
|
% Saturation between min: m and max: M
|
|
sat = @(x, m, M) max(m, min(M, x));
|
|
|
|
|
|
% calculate error diff
|
|
de = (e - prev_e)/Ts;
|
|
prev_e = e;
|
|
|
|
% Normalize e and de
|
|
en = sat(Ke*e, -1, 1);
|
|
den = sat(Kde*de, -1, 1);
|
|
% evaluate FIS
|
|
dun = evalfis(fis, [en den]);
|
|
% De-normalize dun
|
|
du = Kdu * dun;
|
|
|
|
|
|
% Update outputs
|
|
u = du + prev_u;
|
|
prev_u = u;
|
|
|
|
end |