clear
close all

% co = [1    0  0.4
%     0.8  0.2  0.5
%     0.6  0.4  0.6
%     0.4  0.6  0.7
%     0.2  0.8  0.8
%       0    1  0.9];
% 
% set(groot,'DefaultAxesColorOrder',co)
% set(groot,'DefaultAxesLineStyleOrder','-.|*')
% set(groot,'defaultLineLineWidth',2)

% set(groot,'DefaultAxesColorOrder','remove')
% set(groot,'DefaultAxesLineStyleOrder','remove')
% set(groot,'defaultLineLineWidth','remove')

s=tf('s');
% Planta
Gp=1.57/(s*(s+1));
Gplc=feedback(Gp,1);

% figure; step(Gplc)

Gc=(25*s + 1)/(62.5*s + 1);
Glc=feedback(Gc*Gp,1);
figure; step(Glc)

% Calculo del factor de amortiguamiento relativo:
spec_lc_c=stepinfo(Glc);
Mp=spec_lc_c.Overshoot;
xita=-log(Mp/100)/sqrt(log(Mp/100)^2 + pi^2);

% Sistema en tiempo discreto ajustando el periodo de muestreo
RT=[0.05,0.95];
especificaciones=stepinfo(Glc,'RiseTimeLimits',RT);
tr=especificaciones.RiseTime;
Nr=20;
T=tr/Nr;

z=tf('z',T);

Gpd=c2d(Gp,T,'zoh');
% figure; pzmap(Gpd)
Gpd=(1/z)*Gpd;
[Z,P,Kpd,TS] = zpkdata(Gpd,'v');

Gpdlc=feedback(Gpd,1);
figure; step(Gplc,Gpdlc);

% Funcion transferencia compensador atraso de fase discreto          
% Gat(z) = Kc*(z - zc)/(z - pc)

% Especificaciones de proyecto
Mp=4/100;               % Sobrepaso maximo. Mp=exp(-pi.sigma/wd)
ts=10;                  % 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+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

% Polos y ceros de la planta discreta;
PP=pole(Gpd);
ZZ=zero(Gpd);
% Polos de la planta discreta
pp1=PP(1,1);
pp2=PP(2,1);
pp3=PP(3,1);
% Ceros de la planta
zz1=ZZ;
zcd=pp3;     % cero del compensador por cancelacion con polo de la planta

% Diseño del compensador de atraso discreto
% Condicion de angulo
% Vectores de los polos a z1
v1=(pp2-real(z1))+1i*imag(z1);      %fi1 vector desde el polo en z=1
v2=real(z1)+1i*imag(z1);            %fi2 vector desde el polo en z=0
v3=(real(z1)-pp3)+1i*imag(z1);      %fi3 vector desde el polo en z=0.87

% Vectores de los ceros a z1
v4=(real(z1)+abs(zz1))+1i*imag(z1); %tita1 vector desde el cero en z=-0.95
% Vector del cero del compensador a z1
v5=(real(z1)-zcd)+1i*imag(z1);      %tita2 vector desde el cero del compensador en zc=0.87
                                    %tita2 = fi3 y v5 = v3: cancelacion polo-cero
% Vector del polo del compensador a z1
% v6=(real(z1)-pcd)+1i*imag(z1);

% 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;

% Angulo del polo del compensador a z1
% fi4=angle(v6); 

% Angulos de los ceros a z1
tita1=angle(v4); tita1g=tita1*180/pi;
tita2=angle(v5); tita2g=tita2*180/pi;

% fi4 = -pi - tita1 - tita2 + fi1 + fi2 + fi3;
% fi4 = -fi4 + 2*pi;
% fi4g=fi4*180/pi;

fi4g = -(-180 - tita1g - tita2g + fi1g + fi2g + fi3g);
fi4 = fi4g*pi/180;

% Posicion del polo del compensador
pcd=((tan(fi4)*real(z1)) - imag(z1))/tan(fi4);

% Vector del polo del compensador a z1
v6=(real(z1)-pcd)+1i*imag(z1);

% Condicion de modulo: Ganancia del compensador discreto
Kcd = (abs(v1)*abs(v2)*abs(v3)*abs(v6))/(Kpd*abs(v4)*abs(v5));

Gcd = Kcd*((z - zcd)/(z - pcd));

% FT lazo abierto
Glad = Gpd*Gcd;
Gpdlc_c = feedback(Glad,1);

% figure; pzmap(Gpdlc_c)

td=0:T:30-T;
% Comparacion entre sistemas sin compensar y compensado en TD
figure; step(Gpdlc,td); hold on
step(Gpdlc_c,td);
legend('Sistema TD No Compensado','Sistema TD Compensado');

% Comparacion entre sistemas compensados en TC y TD
figure; step(Glc); hold on
step(Gpdlc_c);
legend('Sistema TC Compensado','Sistema TD Compensado');

% Analisis en el dominio de la frecuencia
figure; bode(Glc,Gpdlc,Gpdlc_c); grid
legend('Sistema TC Compensado','Sistema TD No Compensado','Sistema TD Compensado');

figure; margin(Gpd); 
legend('Sistema TD No Compensado')
figure; margin(Glad); grid
legend('Sistema TD Compensado');

% Analizando los márgenes de estabilidad, se observa que se ganan
% importantes aumentos en el margen de ganancia como en el margen de fase.
% Por otro lado, se reduce un poco el ancho de banda respecto a la
% compensación del sistema en tiempo continuo, lo que se explica por la
% característica del compensador (ser de atraso de fase) y la amplia
% separación entre el cero y el polo del mismo; imponiendo un aporte de
% fase negativo importante que afecta principalmente al tiempo de subida de
% la respuesta. No obstante, se consigue disminuir el tiempo de
% asentamiento de esta respuesta en tiempo discreto, respecto de la
% respuesta del sistema compensado en tiempo continuo, lo que desde el
% punto de vista del rechazo a las perturbaciones es provechoso para el
% proceso.

% Trazado de la accion de control
[yk,td] = step(Gpdlc_c,td);
rk = ones(length(td),1);
ek = rk - yk;

figure; lsim(Gcd,ek,td); grid

% En cuanto a la acción de control necesaria para la peor condición, cuando
% la señal de error cambia en forma de escalón a partir de la aplicación de
% un escalón de referencia, se observa que su valor máximo no llega a 0,2
% p.u. (1,81).