function Simulation_d_un_systeme_de_commande_multivariable_discret();
% Simulation d'un système de commande multivariable discret
% Procédé: système 2X2
% Contrôleur décentralisé: 2 PI
% A. Desbiens, mars 2015


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% AVEC UNE BOUCLE for ET DES FONCTIONS DE TRANSFERT

Ts=0.1; %periode d'echantillonnage

%%% PROCÉDÉ

% en continu       
Gp11=tf(1,[1 1]);        
Gp12=tf(0.5,[0.5 1]);  
Gp21=tf(0.5,[1 1]);        
Gp22=tf(2,[2 1]);       

%discrétisation du procédé par la méthode 'zoh' (bloqueur d'ordre zéro)
Gp11=c2d(Gp11,Ts,'zoh');
Gp12=c2d(Gp12,Ts,'zoh');
Gp21=c2d(Gp21,Ts,'zoh');
Gp22=c2d(Gp22,Ts,'zoh');
  

%%% CONTRÔLEUR DÉCENTRALISÉ

% en continu
Gc1=tf([1 1],[1 0]);        
Gc2=tf(0.5*[2 1],[2 0]);        

%discrétisation du procédé par la méthode 'tustin' (bilinéaire)
Gc1=c2d(Gc1,Ts,'tustin');
Gc2=c2d(Gc2,Ts,'tustin');


%%% INITALISATION

% num et den des fonctions de transfert
nump11=[Gp11.num{:}(2:end) 0]; % NOTE IMPORTANTE: on enlève le retard de discrétisation du bloqueur d'ordre zéro dans la définition du procédé (voir explicaion plus bas)
denp11=Gp11.den{:};
nump12=[Gp12.num{:}(2:end) 0]; 
denp12=Gp12.den{:};
nump21=[Gp21.num{:}(2:end) 0]; 
denp21=Gp21.den{:};
nump22=[Gp22.num{:}(2:end) 0]; 
denp22=Gp22.den{:};
numc1=Gc1.num{:};
denc1=Gc1.den{:};
numc2=Gc2.num{:};
denc2=Gc2.den{:};

% vecteur dans lesquel seront gardés les données passées nécessaires pour le calcul de l'équation récurrente
xp11=zeros(max(length(denp11),length(nump11))-1,1); 
xp12=zeros(max(length(denp12),length(nump12))-1,1);
xp21=zeros(max(length(denp21),length(nump21))-1,1);
xp22=zeros(max(length(denp22),length(nump22))-1,1);
xc1=zeros(max(length(denc1),length(numc1))-1,1);
xc2=zeros(max(length(denc2),length(numc2))-1,1);

N=150;                      % nombre de points a simuler
u1=0;                       % sortie des controleurs a t=0
u2=0;
data=zeros(N,6);            % storage des resultats de simulation [u1 u2 y1 y2 r1 r2];
r1=[0 ones(1,N-1)];         % valeurs des consignes
r2=[zeros(1,N-75) 0.8*ones(1,75)];

                                               
%%% SIMULATION                                               

for i=1:N  
    %sorties du procede
    [y11,xp11]=filter(nump11,denp11,u1,xp11); %y(k): on utilise le u de l'itération précédente, d'où l'enlèvement d'un retard
    [y12,xp12]=filter(nump12,denp12,u2,xp12);
    [y21,xp21]=filter(nump21,denp21,u1,xp21);
    [y22,xp22]=filter(nump22,denp22,u2,xp22);
    y1=y11+y12;
    y2=y21+y22;
    %calcul des erreurs
    err1=r1(i)-y1;
    err2=r2(i)-y2;
    %calcul des actions
    [u1,xc1]=filter(numc1,denc1,err1,xc1);
    [u2,xc2]=filter(numc2,denc2,err2,xc2);   
    %enregistrement
    data(i,:)=[u1 u2 y1 y2 r1(i) r2(i)];
end

%graphes
plotfigure(1,Ts,data);
    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% AVEC UNE BOUCLE for ET LA REPRÉSENTATION D'ÉTAT

%%% PROCEDE

% représentation d'état
Gpss=ss([Gp11 Gp12; Gp21 Gp22]);


%%% CONTROLEUR

% représentation d'état
Gcss=ss([Gc1 0; 0 Gc2]);

                                               
%%% SIMULATION                                               

%initialisation pour la simulation
u=[0;0];                    % sortie des controleurs a t=0
nx=length(Gpss.A);            % dimension du vecteur d'etat du procédé
xp=zeros(nx,1);             % vecteur d'etat du procédé
nx=length(Gcss.A);            % dim du vecteur d'etat du contrôleur
xc=zeros(nx,1);             % vecteur d'etat du contrôleur
data=zeros(N,6);            % storage des resultats de simulation [u1 u2 y1 y2 r1 r2];
r=[r1; r2];                  % valeurs des consignes

for i=1:N
    % NOTE IMPORTANTE: Equation d'observation d'abord puis l'équation d'état
    %sorties du procede
    y=Gpss.c*xp;
    %calcul des erreurs
    err=r(:,i)-y;
    %calcul des actions
    u=Gcss.c*xc+Gcss.d*err;
    %mnise a jour des etats
    xp=Gpss.a*xp+Gpss.b*u;    
    xc=Gcss.a*xc+Gcss.b*err;    
    %enregistrement
    data(i,:)=[u' y' r(:,i)'];
end

%graphes
plotfigure(2,Ts,data);
    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DIRECTEMENT AVEC LE SYSTÈME COMPLET

% Régulateur
Gc=Gcss;  % On pourrait aussi utiliser Gc=[Gc1 0; 0 Gc2];
% Procédé
Gp=Gpss; % On pourrait aussi utiliser Gp=[Gp11 Gp12; Gp21 Gp22];

% Le système entre les consignes et les sorties est:
Hry=feedback(Gc*Gp,eye(2));
% Sa réponse est:
y=lsim(Hry,r);

% Le système entre les consignes et les commandes est:
Hru=feedback(Gc,Gp);
% Sa réponse est:
u=lsim(Hru,r);

data=[u y r'];

%graphes
plotfigure(3,Ts,data);



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot figure

function plotfigure(num,Ts,data);

N=length(data);
t=Ts*(0:N-1);
figure(num);
subplot(211);
hold on;
stairs(t,data(:,1),'r-');
stairs(t,data(:,2),'b-');
hold off;;
ylabel('Commandes');
subplot(212);
hold on;
stairs(t,data(:,3),'r-');
stairs(t,data(:,4),'b-');
stairs(t,data(:,5),'r--');
stairs(t,data(:,6),'b--');
hold off;
ylabel('Consignes et Sorties');