%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Sous-Programme calcul_marges_2.m %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Mmi,Mmo,Mmci,Mmco]=calcul_marges_2(A,B,C,E,Kp,Ki,Kt,n,m,p,q)

matrice_A=A;
matrice_B=B;
matrice_C=C;
matrice_E=E;
nombre_n=n;
nombre_m=m;
nombre_p=p;
nombre_q=q;
matrice_Kp=Kp;
matrice_Ki=Ki;
matrice_Kt=Kt;

% Représentation d'état du système à régler 
sys=ss(A,B,C,zeros(nombre_m,nombre_q));

% Représentation d'état du régulateur K
Ab=matrice_A-matrice_B*matrice_Kp-matrice_Kt*matrice_C;
reg1=ss(Ab,matrice_Kt,matrice_Kp,zeros(nombre_m,nombre_q));
reg2=ss(Ab,matrice_B*matrice_Ki*matrice_E,-matrice_Kp,matrice_Ki*matrice_E);
integrateur=ss(zeros(nombre_m,nombre_m),eye(nombre_m,nombre_m),eye(nombre_m,nombre_m),zeros(nombre_m,nombre_m));
reg2=reg2*integrateur;
reg=reg1+reg2;

% Représentation d'état du système corrigé en boucle ouverte, vues de l'entrée et de la sortie, Li et Lo
Li=reg*sys;
Lo=sys*reg;

% Lieux de Bode de Li(s) et Lo(s)
if nombre_m+nombre_p==2 
    bode(Li);grid;title('Bode de L(s), FTBO du système corrigé - pour les marges: click droit --> Characteristics --> All stability margins');
else
end

% Représentation d'état des matrices de sensibilité Si et So
Si=inv(eye(nombre_m,nombre_m)+Li);
So=inv(eye(nombre_p,nombre_p)+Lo);

% Représentation d'état des matrices de sensibilité complémentaire Ti et To
Ti=eye(nombre_m,nombre_m)-Si;
To=eye(nombre_p,nombre_p)-So;

% Diagrammes de Bode (éventuellement multivariables) de Si, So, Ti et To
if nombre_m+nombre_p==2 
    figure; bodemag(Si); grid; title('Bode de S(s), fonction de sensibilité du système corrigé');
    figure; bodemag(Ti,To,'--'); grid; title('Bode de T(s), fonction de sensibilité complémentaire du système corrigé');
else
    sigma(Si,So,'--'); grid; title('Bode multivariable de Si (trait plein) et de So (trait pointillé)');
    figure; sigma(Ti,To,'--'); grid; title('Bode multivariable de Ti (trait plein) et de To (trait pointillé)');
end

% Calcul des marges de module
Mmi=1/norm(Si,inf);
Mmo=1/norm(So,inf);
Mmci=1/norm(Ti,inf);
Mmco=1/norm(To,inf);