%%
% Levantamento da planta

close all
clear all

% Função de transferência entre resistência e temperatura (sensor no furo
% 4) sem tempo morto
num_14_sdt=[0.1358]; den_14_sdt=[3.7599*0.0379 (3.7599+0.0379) 1];
sys_14_sdt = tf(num_14_sdt,den_14_sdt);
% adição do tempo morto
[num_14_dt,den_14_dt] = pade(0.096,1);
sys_14_dt=tf(num_14_dt, den_14_dt);
sys_14=series(sys_14_dt,sys_14_sdt);

% Função de transferência entre ventilador e temperatura (sensor no furo
% 4) sem tempo morto
num_24_sdt=[0.1132]; den_24_sdt=[4.4903*0.5896 (4.4903+0.5896) 1];
sys_24_sdt = tf(num_24_sdt,den_24_sdt);
% adição do tempo morto
[num_24_dt,den_24_dt] = pade(0.5,1);
sys_24_dt=tf(num_24_dt, den_24_dt);
sys_24=series(sys_24_dt,sys_24_sdt);

% Função de transferência entre ventilador e vazão de massa (sem tempo
% morto)
num_3_sdt=[12.6470]; den_3_sdt=[0.3621*0.068 (0.3621+0.068) 1];
sys_3_sdt = tf(num_3_sdt,den_3_sdt);
% adição do tempo morto
[num_3_dt,den_3_dt] = pade(0.24,1);
sys_3_dt=tf(num_3_dt, den_3_dt);
sys_3=series(sys_3_dt,sys_3_sdt)

% Função de transferência entre flap e temperatura (sem tempo
% morto)
num_44_sdt=[1.107]; den_44_sdt=[7.675*0.1528 (7.675+0.1528) 1];
sys_44_sdt = tf(num_44_sdt,den_44_sdt);
% adição do tempo morto
[num_44_dt,den_44_dt] = pade(0.78,1);
sys_44_dt=tf(num_44_dt, den_44_dt);
sys_44=series(sys_44_dt,sys_44_sdt);

% Função de transferência entre flap e vazão de massa (sem tempo
% morto)
num_5_sdt=[0.8117]; den_5_sdt=[0.0322*0.0319 (0.0322+0.0319) 1];
sys_5_sdt = tf(num_5_sdt,den_5_sdt);
% adição do tempo morto
[num_5_dt,den_5_dt] = pade(0.08,1);
sys_5_dt=tf(num_5_dt, den_5_dt);
sys_5=series(sys_5_dt,sys_5_sdt);

% Faz a associação em paralelo dos sistemas para as entradas controláveis
% (ou seja, sem o FLAP).
%
% As duas funções de transferência que caracterizam o sistema são (para
% entradas diferentes)
[num_11,den_11] = tfdata(sys_14,'v');
[num_12,den_12] = tfdata(sys_24,'v');
num_21=0;den_21=1;
[num_22,den_22] = tfdata(sys_3,'v');


%%
% Montagem do modelo matemático com tempo morto aproximado

% Determina a matriz de funções de transferência
nums = {num_11 num_12; num_21 num_22};
dens = {den_11 den_12; den_21 den_22};

% Monta o sistema com as funções de transferência especificadas
sys=tf(nums,dens);


% Encontra uma representação em espaço de estados para o sistema
[A,B,C,D] = ssdata(sys);
syss=ss(A,B,C,D);
psys=prescale(syss);   % Faz um escalamento do modelo para ficar melhor 
                       % condicionado

% plota o gráfico de valores singulares do sistema completo
figure(1)
sigma(psys); hold on
legend('Planta')
grid

% calcula os autovalores 
 eig(A)

 % determina se o sistema é controlável
 rank(ctrb(A,B))
 
 % determina se o sistema é observável
 rank(obsv(A,C))
 
 
 % Imprime as matrizes no formato do Latex
 
 %A_s=sym(A);
 %A_s=vpa(A_s,4);
 %latex(A_s)
 
 %B_s=sym(B);
 %B_s=vpa(B_s,4);
 %latex(B_s)
 
 %C_s=sym(C);
 %C_s=vpa(C_s,4);
 %latex(C_s) 
 
 
 G_pl=tf(psys);
 

% Calcula a Norma H infinito para a planta 
 20*log10(hinfnorm(G_pl))
 
 % Resposta ao Degrau Unitário para todos os canais
 
 figure(2) 
 step(G_pl);
 legend('Planta')
 
 
 % Plota os pólos e zeros da planta
 
 figure(3)
 pzmap(G_pl); hold on
 legend('Planta')
 
 %%
 % Fechando a malha com controlador unitário k=0.05 a 0.25
 
 
%  for k=0.02:0.01:0.25,
%     sys_mf_k_1 = feedback(k*psys,eye(2),-1); 
%     sys_sen_k_1 = feedback(eye(2),k*psys,-1);
%     figure(10)
%     sigma(sys_mf_k_1); hold on; 
%     figure(11)
%     step(sys_mf_k_1); hold on
%     figure(12)
%     pzmap(sys_mf_k_1); hold on
%     figure(13)
%     sigma(sys_sen_k_1); hold on
%     20*log10(hinfnorm(sys_sen_k_1))
% 
%  end
%  %% Diagrama de Nyquist MIMO
%  
%  for kk=0.05:0.05:.25,
%     nyqq=[];
%     Acl=A-kk*B*[(eye(2)-D)^(-1)]*C;
%     ww=[1 1.5 2 2.5 3 3.5 4 5 6 7 8 9];
%     www=[9 8 7 6 5 4 3.5 3 2.5 2 1.5 1];
%     w=[-100*www, -10*www, -www, -0.1*www, -0.01*www, -0.001*www, 0.001*ww, 0.01*ww, 0.1*ww, ww, 10*ww, 100*ww];
%     lw=length(w);
%     for k=1:1:lw,
%       nyq = det(eye(2)+D)*(det(i*w(k)*eye(9)-Acl)/det(i*w(k)*eye(9)-A));
%       nyqq=[nyqq;nyq]; hold on
%     end
%  
%     figure(14)
%     plot(nyqq); hold on
%  
%  end 
%   
  %% Solução H infinito
 
  
     % Funções peso
     
     w0=1;                   % Banda passante desejada em malha fechada
     A=1/100;                % Constante de erro estacionário e atenuação 
                             % de disturbio nas baixas frequências 
     M=2 ;                   % Limite superior na norma H infinito de S e T
     s=tf('s');              % Variável de Laplace Simbólica
     W1=(s/M+w0)/(s+w0*A);   % Funçao peso de desempenho
     W2=[.01];                % Função peso de controle
     W3=0;                   % Função peso da sensibilidade complementar

     
     % Determina a planta aumentada para o problema de rastreamento (como
     % visto em aula) e encontra uma representação em espaço de estados
     P = augw(G_pl,W1*eye(2),W2*eye(2),W3*eye(2));

     
     % Calcula o controlador H infinito
     [K,CL,GAM,INFO] = hinfsyn(P);

     
     % Plot os valores singulares do controlador projetado
     figure(4)
     sigma(K)
     legend('Controlador')
     
     % Determina a matriz de funções de transferência em malha aberta
     % (MFTMA)
     L=G_pl*K;
     
     % Plota os valores singulares da MFTMA
     figure(1)
     sigma(L)
     legend('Planta','malha aberta')
     
     % Calcula e plota os valores singulares da  função sensibilidade 
     % e o inverso da função peso
     figure(5) 
     S2=feedback(eye(2),L,-1);
     sigma(S2); hold on
     sigma(inv(W1))  % Inverso da função peso
     legend('S','1/W1')
     
     % Calcula e plota os valores singulares da função sensibilidade
     % complementar
     figure(6)
     T2=feedback(L,eye(2),-1);
     sigma(T2)
     legend('T')
     
     % Calcula a resposta ao degrau do sistema em malha fechada
     figure(7)
     step(T2)
     legend('degrau em T')
     
     % Resposta ao degrau de disturbio
     figure(8)
     step(S2)
     legend('degrau em S')