close all;
clear all;
clc;
% ft planta
ka=2.1;
k1=0.77;
kf=-0.000576;
Bf=1/kf;
L=0.52;
m=0.0226;
R=21;
k2=39.6;
num=-ka*k1*Bf;
den=[L*m R*m -k2*L -k2*R];
sys1=tf(num,den);
sys1ss=ss(sys1);
[as1,bs1,cs1,ds1]=ssdata(sys1ss);

% funçoes W
zw1=[];
zw1=zw1';
pw1=[-.1];
pw1=pw1';
kw1=2.25;
[numw1,denw1] = zp2tf(zw1,pw1,kw1);
sysw1=tf(numw1,denw1);

zw2=[-20];
zw2=zw2';
pw2=[-150];
pw2=pw2';
kw2=.0045;
[numw2,denw2] = zp2tf(zw2,pw2,kw2);
sysw2=tf(numw2,denw2);

zw3=[-20];
zw3=zw3';
pw3=[-190];
pw3=pw3';
kw3=.095;
[numw3,denw3] = zp2tf(zw3,pw3,kw3);
sysw3=tf(numw3,denw3);

% METODO 1: projeto utilizando a funcao augtf
[TSS_2]=augtf(sys1,sysw1,sysw2,sysw3);
[SS_CP,SS_CL,HINFO,TSS_K]=hinf(TSS_2);
% SS_CP 'e a representacao do controlador na forma espaco de estados
% encontrando a funcao de transferencia para comparacao com o outro metodo
ft1=tf(SS_CP);
zeropolo1=zpk(SS_CP);

% METODO 2: projeto utilizando o simulink
% chamar o arquivo simulink com a representacao do sistema (o arquivo deve estar no diretorio em uso)
run PAugexpl2;
% obter a representacao espaco de estados do sistema representado pelo arquivo
[A,B,C,D]=linmod('PAugexpl2');

% separar as matrizes, colocando na forma:

%   | A  | B1  B2  |
%   |----|---------|
%   | C1 | D11 D12 |
%   | C2 | D21 D22 |

B1=B(:,1);
B2=B(:,2);
C1=C(1:3,:);
C2=C(4,:);
D11=D(1:3,1);
D12=D(1:3,2);
D21=D(4,1);
D22=D(4,2);

% encontrar o controlador a partir das matrizes da planta aumentada
[acp,bcp,ccp,dcp,acl,bcl,ccl,dcl,HINFO,TSS_K]=hinf(A,B1,B2,C1,C2,D11,D12,D21,D22);
% colocar as matrizes espaco de estado em um sistema 
ss1=ss(acp,bcp,ccp,dcp);
% encontrar a funcao de transferencia para comparacao com o outro metodo
ft2=tf(ss1);
zeropolo2=zpk(ss1);

disp ('*********************************************************************')
disp ('funcao de transferencia utilizando a funcao augtf:')
zeropolo1
disp ('*********************************************************************')
disp ('funcao de transferencia utilizando o simulink:')
zeropolo2




%sistema final
%KG
sysf1=series(sys1,ft1);
%T
sysf2=feedback(sysf1,1);
%1+KG
sysf3=parallel(sysf1,1);

% verificacao das condicoes
figure(1)
bodemag(inv(sysf3),inv(sysw1))
legend('S','1/w1')
figure(2)
bodemag(sysf1,inv(sysw2))
legend('KS','1/w2')
figure(3)
bodemag(sysf2,inv(sysw3))
legend('T','1/w3')




% OBS: foram encontradas as funcoes de transferencia dos controladores apenas para comparacao, pois 
%      a representacao em espaco de estados nao 'e unica. Nao 'e recomendavel mudar de representacao,
%      pois podem ser introduzidos erros, principalmente para sistemas de alta ordem

%---------------------------------------------------------------------------------------------------
% Daniel Siqueira