clear
close all

s=tf('s');

% Planta
Gp=100/(s*(s+2.5));
syslcc_nc=feedback(Gp,1);
figure; step(syslcc_nc);
figure; pzmap(syslcc_nc)


% Especificaciones de proyecto
% El numero deseado de muestras en un periodo es de:
Nm=50;

% El tiempo de asentamiento del sistema compensado debe ser menor o igual a:
ts=1;                   % ts=4/sigma
sigma=4/ts;             % Parte real de los polos dominantes de lazo cerrado
% El sobrepaso maximo deseado debe ser menor o igual a 3%
Mp=3/100;               % Sobrepaso maximo del 5%. Mp=exp(-pi.sigma/wd)
wd=-pi*sigma/log(Mp);   % Parte imaginaria de los polos dominantes de lazo cerrado
T=2*pi/(Nm*wd);         
s1=-sigma+1i*wd;         % Polos dominantes de lazo cerrado
s2=-sigma-1i*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

z=tf('z',T);
Gpd=c2d(Gp,T,'zoh');
figure; pzmap(Gpd);


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);    % polo planta
pp2=PP(2,1);    % polo planta
zz1=abs(ZZ);    % cero planta

% Proyecto Compensador PD discreto
% Condicion de angulo
% Vectores de los polos y ceros a z1
v1=(real(z1)+zz1) + 1i*imag(z1);    %tita1: cero de la planta
v2=(pp1-real(z1)) + 1i*imag(z1);    %fi1: polo de la planta en z=1

% Vector del polo del compensador a z1
%v3=(real(z1)-b) + j*imag(z1);     %tita_b: polo del compensador

% Angulos de los polos y ceros a z1
tita1=angle(v1); tita1g=tita1*180/pi;
fi1=pi-angle(v2); fi1g=fi1*180/pi;

% Obtención del parametro "b" a partir de la condición de angulo
% tita_b=arctg[imag(z1)/(real(z1) - b)];
alfa=-pi - fi1 + tita1;

b=((real(z1)*tan(alfa))-imag(z1))/tan(alfa);
v3=(real(z1)-b) + 1i*imag(z1);

% Condicion de modulo
VAbs=0.05972*abs(v1)/(abs(v2)*abs(v3));
K=1/VAbs;

% Funcion transferencia PI discreto          
% Gpi(z)=K*(z+a)/(z-1)
a=pp2;
Gcd=K*(z-a)/(z-b);

% FT lazo abierto
Glad=Gpd*Gcd;
figure; pzmap(Glad);

syslc_c=feedback(Glad,1);
figure; pzmap(syslc_c)

t=0:T:4;
figure; step(syslc_c,t);
hold on
step(syslc_nc,t);
legend('compensado','no compensado');
 
figure; nyquist(Gpd);
figure; nyquist(Glad);

t=0:T:5;
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

r=ones(length(t),1);
ystep=lsim(syslc_c,r,t);
error_step=r - ystep;
figure; lsim(Gcd,error_step,t)