clear
close all

s=tf('s');
% Planta
Gp=1080/(s*(s+6)*(s+18));
T=0.06;
z=tf('z',T);

% Especificaciones de proyecto
Mp=25/100;              % Sobrepaso maximo del 25%. Mp=exp(-pi.sigma/wd)
ts=1.8;                 % Tiempo de establecimiento 1,8seg. ts=4/sigma
sigma=4/ts;             % Parte real de los polos dominantes de lazo cerrado
wd=-pi*sigma/log(Mp);   % Parte imaginaria de los polos dominantes de lazo cerrado
s1=-sigma+j*wd;         % Polos dominantes de lazo cerrado
s2=-sigma-j*wd;         % complejos conjugados en el dominio continuo

z1=exp(s1*T);           % Polos dominantes de lazo cerrado
z2=exp(s2*T);           % complejos conjugados en el dominio discreto

Gpd=c2d(Gp,T);
[Z,P,Kpd] = zpkdata(Gpd,'v');

figure; pzmap(Gpd)
hold on; plot(real(z1),imag(z1),'x'); %axis([-3 2 -1 1])

syslc_nc=feedback(Gpd,1);
% figure; step(syslc_nc);
% figure; pzmap(syslc_nc)

% Polos y ceros de la planta discreta;
PP=pole(Gpd);
ZZ=zero(Gpd);
pp1=PP(1,1);
pp2=PP(2,1);
pp3=PP(3,1);
zz1=ZZ(1,1);
zz2=ZZ(2,1);

% Proyecto Compensador PD discreto
% Condicion de angulo
% Vectores de los polos a z1
v1=(pp1-real(z1))+j*imag(z1);      %fi1
v2=(real(z1)-pp2)+j*imag(z1);      %fi2
v3=(real(z1)-pp3)+j*imag(z1);      %fi3
v4=real(z1)+j*imag(z1);            %fi4
v5=real(z1)+j*imag(z1);

% Vectores de los ceros a z1
%v6=(real(z1)-a)+j*imag(z1);      %tita1
v7=(real(z1)-zz2)+j*imag(z1);     %tita2
v8=(real(z1)-zz1)+j*imag(z1);     %tita3

% Angulos de los polos a z1
fi1=pi-angle(v1); fi1g=fi1*180/pi;
fi2=angle(v2); fi2g=fi2*180/pi;
fi3=angle(v3); fi3g=fi3*180/pi;
fi4=angle(v4); fi4g=fi4*180/pi;
fi5=angle(v5); fi5g=fi4*180/pi;

% Angulos de los ceros a z1
%tita1=angle(v6);
tita2=angle(v7); tita2g=tita2*180/pi;
tita3=angle(v8); tita3g=tita3*180/pi;

tita1=pi + (fi1 + fi2 + fi3 + fi4 + fi5) - (tita2 + tita3);
xx=tan(tita1);

a=(imag(z1)-(xx*real(z1)))/(-xx);
v6=(real(z1)-a)+j*imag(z1);

% Condicion de modulo
yy=Kpd*(abs(v6)*abs(v7)*abs(v8))/(abs(v1)*abs(v2)*abs(v3)*abs(v4)*abs(v5));
K=1/yy;

% Para obtener Kp y Kd
Kp=K*(1-a);
Kd=a*Kp*T/(1-a);
Gpdd=((Kp*T+Kd)/T)*((z-a)/z);

Gcd=K*(z-a)/z;
Cz=2*z/(z+1);
% FT lazo abierto
Glad=Gpd*Gcd*Cz;

syslc_nc=feedback(Gpd,1);
syslc_c=feedback(Glad,1);
% figure; pzmap(syslc_c)
figure; step(syslc_c);
hold on
step(syslc_nc);
legend('compensado','no compensado');
 
figure; margin(Gpd);
figure; margin(Glad);

% t=0:T:10;
% rampa=t;
% figure; yr=lsim(syslc_c,rampa,t); plot(t,rampa,t,yr); grid
% error=(rampa-yr')*100; figure; plot(t,error,'k'); grid