%% Ejemplo diseño controladores
%% Planta G(s)=1/(s*(s+1))
%% Periodo de muestreo Ts=0.2
%% Coef amortiguamiento 0.5 frc natural 4.5345
%% Red de adelanto
%% Datos

num=1;			% Planta
den=[1 1 0];

capa=0.5;		% Especificaciones
wn=4.5345;

Ts=0.2;		% Periodo de muestreo

% Polo dominante

s1=-capa*wn+wn*sqrt(1-capa*capa)*i;

% Diseño controlador continuo

zc=-1;	% Posciono el polo de cancelación

numc=[1 -zc];

ang=angle(polyval(num,s1)/polyval(den,s1))*180/pi; 
if ang<0			% Contribucion angular Compensador

	cang=ang+180;
else
	cang=180-ang;
end 

angnum=angle(polyval([1 -zc],s1))*180/pi;
zp=-real(s1)+imag(s1)/tan((angnum-cang)*pi/180);
denc=[1 zp];

kc=abs(polyval(den,s1))*abs(polyval(denc,s1))/((abs(polyval(num,s1))*abs(polyval(numc,s1))));

fprintf('Controlador continuo\n');
printsys(kc*numc,denc);

[numcc,dencc]=cloop(kc*conv(num,numc),conv(den,denc));

step(numcc,dencc,0:0.05:2);

pause
% controlador continuo con mantenedor a entrada

num1=1;
den1=conv(den,[Ts/2 1]);

% Diseño controlador continuo

zc=-1;	% Posciono el cero de cancelación

numc=[1 -zc];

ang=angle(polyval(num1,s1)/polyval(den1,s1))*180/pi; 
if ang<0			% Contribucion angular Compensador

	cang=ang+180;
else
	cang=180-ang;
end 

angnum=angle(polyval([1 -zc],s1))*180/pi;
zp=-real(s1)+imag(s1)/tan((angnum-cang)*pi/180);
denc=[1 zp];

kc=abs(polyval(den1,s1))*abs(polyval(denc,s1))/((abs(polyval(num1,s1))*abs(polyval(numc,s1))));

fprintf('Controlador continuo con mantenedor a la entrada\n');
printsys(kc*numc,denc);

%% Discretizamos


[numcd,dencd]=c2dm(kc*numc,denc,Ts,'matched');

fprintf('Controlador discreto equivalente al continuo\n');
printsys(numcd,dencd,'z');

% Calculo G(z)

[numd,dend]=c2dm(num,den,Ts,'zoh');

% Lazo cerrado

[numdt,dendt]=cloop(conv(numd,numcd),conv(dend,dencd));

hold on
dstep(numdt,dendt,0:Ts:2);

pause
%% Calculo en digital directamente

z1=exp(Ts*s1);	% Polo dominante en el plano z

% Diseño controlador discreto

zdc=exp(-1*Ts);	% Posiciono el cero de cancelación

ndc=[1 -zdc];

ang=angle(polyval(numd,z1)/polyval(dend,z1))*180/pi 
if ang<0			% Contribucion angular Compensador

	cang=ang+180;
else
	cang=180-ang;
end 

angnum=angle(polyval([1 -zdc],z1))*180/pi;
zdp=-real(z1)+imag(z1)/tan((angnum-cang)*pi/180);
ddc=[1 zdp];

kdc=abs(polyval(ddc,z1))*abs(polyval(dend,z1))/((abs(polyval(ndc,z1))*abs(polyval(numd,z1))));

fprintf('Controlador discreto diseño en z\n');
printsys(kdc*ndc,ddc,'z');

[numdt2,dendt2]=cloop(kdc*conv(ndc,numd),conv(ddc,dend));


dstep(numdt2,dendt2,0:Ts:2);

hold off

