% ------------------ SISTEMAS DE CONTROL 2 ------------------- 
% Ejemplo 2: Método del Lugar Geométrico de las Raices.
% Controlador de Adelanto de Fase.
% ------------------------------------------------------------
clear
close all
clc
s = tf('s');
         
% Planta y especificaciones deseadas:
Gp = 1/(s*(s + 2)); 
Mp = 2/100;               % Sobrepaso para el 2do diseño 
ts = 2;

% Determinación y verificación del periodo de muestreo:
% -------------------------------------------------------------------------
sigma_des = 4.5/ts;       % Parte real de los polos dom. a LC controlado.
sigma = 4;                % Sigma para ts menor a 2 seg y conseguir reducido essv
wd = -pi*sigma/log(Mp);   % Parte imaginaria de los polos dominantes a LC controlado.
Nd = 20;                  % Nº de muestras para el 1er. periodo de osc.
Td = 2*pi/wd;             % Periodo de oscilación de los polos complejos conjugados a LC
T = Td/Nd;                % Periodo de muestreo.
fs = 1/T;                 % Frecuencia de muestreo.

% Para verificar el valor de T trazar la respuesta al escalón a LC
% de la planta en TC y de la planta en TD
z = tf('z',T);                  % Define vaiable z para FTs.
Gpd = c2d(Gp,T);                % Discretiza Gp con ZOH y el T determinado.
Glc = feedback(Gp,1);           % FTLC sin controlador.
Glcd = feedback(Gpd,1);         % FTLC discreta sin controlador.
figure; step(Glc,Glcd); 
axis([0 12 0 1.2]); grid;
title('Respuesta al escalón a LC sin controlador');

% Como se aprecia, la respuesta del sistema discretizado "sigue" a la 
% respuesta del sistema continuo. Por lo cual el periodo T para Nd = 10 es adecuado.

%% Diseño del controlador en tiempo discreto:
% -------------------------------------------------------------------------
% Obtener la FT de la planta en tiempo discreto
% con atraso de 1 periodo de muestreo para modelar el tiempo Ta de
% implementación digital
Gpda = zpk(z^-1*Gpd);
hold on; step(feedback(Gpda,1))
legend('LC Sistema Continuo','LC Sistema Discreto','LC Sistema Discreto con atraso T');
%% Polos dominantes deseados en el plano-s y en el plano-z con z =  exp(s*T)
s1 = -sigma + 1i*wd;   pz1 = exp(s1*T);    
s2 = -sigma - 1i*wd;   pz2 = exp(s2*T);   

% Considerando que el controlador posee la estructura Gc(z)= Kc*(z-a)/(z-b) y
% que el cero "a" cancela el polo dominante de Gpda la FTLA para diseño resulta:

%          Kc       q *(z + zplanta)
% Glad = ----- * --------------------
%        (z-b)         z (z-1)

%% Se extraen ceros, polos y ganancia de la FT de LA:
[zGpda,pGpda,kGpda] = zpkdata(Gpda,'v');   
 q  = kGpda;                           
z1  = zGpda(1);
p1  = pGpda(1); p2 = pGpda(2); p3 = pGpda(3);
% p3 se cancela con el cero del compensador por lo tanto no es
% necesario tenerlo en cuenta en el cáculo del aporte de fase ni de magnitud 
% p4 = b  

%% Vectores desde los ceros y polos de Glad al polo deseado pz1:
Vz1 = pz1 - z1; modVz1 = abs(Vz1); tita1 = angle(Vz1)*180/pi;
Vp1 = pz1 - p1; modVp1 = abs(Vp1);   fi1 = angle(Vp1)*180/pi;
Vp2 = pz1 - p2; modVp2 = abs(Vp2);   fi2 = angle(Vp2)*180/pi;
% Vp4 Podrá calcularse una vez obtenido "b".

%% Se calcula la deficiencia de fase de la planta en el punto pz1
def_fase = tita1 - fi1 - fi2;

%% Aplicar la CONDICION DE FASE para hallar el polo "b" del controlador: 
% ángulo(q) + tita1 - (fi1 + fi2 + fi4) = ángulo(-1/Kc)
% Como q > 0 ==> ángulo(q) = 0. Además, como Kc > 0 y la  
% def_fase es negativa, el ángulo(-1/Kc) debe ser igual a -180º.
% Con esto, el ángulo fi4 del polo del compensador resulta:
fi4 = def_fase + 180;
% Utilizando la función evalfr y teniendo en cuenta la cancelación
% del polo p3 de la planta con el cero del compensador: 
angulo_b = angle(evalfr(Gpda*(z - pGpda(3)),pz1))*180/pi + 180;
%% El angulo_b debe ser igual a fi4. Es otra forma de calcular el aporte de fase.

%% A partir de aquí se calcula la posición del polo b del compensador
%% La posición de b sobre el eje real del plano-z dependerá del ángulo de fi4
%% el cual varía según el periodo T obtenido
if Nd == 10
    b = imag(pz1)/tan(fi4*pi/180) - real(pz1);
end
if Nd == 20
    b =  real(pz1) - imag(pz1)/tan(fi4*pi/180);
end
p4 = b; 

%% Aplicando la CONDICION DE MAGNITUD para hallar la ganacia "Kc" del controlador: 
% (q*abs(Vz1))/(abs(Vp1)*abs(Vp2)*abs(Vp4)) = abs(-1/Kc)

% Se obtiene el vector del polo b del compensador a pz1 el cual variará
% segun el polo se encuentre en el lado derecho o en el lado izquierdo del
% plano-z
if Nd == 10
    Vp4 = pz1 + p4;
end
if Nd == 20
    Vp4 = pz1 - p4;
end
  
%% Finalmente, despejando Kc de la condición de magnitud, se tiene:
Kc = (abs(Vp1)*abs(Vp2)*abs(Vp4))/(q*abs(Vz1));

%% Recordando que el cero del compensador cancela al polo p3 de la planta 
%% el controlador de adelanto de fase resulta:
a = pGpda(3);
if Nd == 10
    Gcd = Kc*(z - a)/(z + b);
end
if Nd == 20
    Gcd = Kc*(z - a)/(z - b);
end

%% Validación del diseño: Respuesta a LC con el controlador diseñado
Glad = zpk(minreal(Gcd*Gpda));                   % FTLA con controlador
Glcd = zpk(minreal(feedback(Glad,1)));           % FTLC con controlador
figure; step(feedback(Gpd,1),Glcd,9); grid; % 
title('Respuesta al escalón del sistema a LC');
legend('Sistema Discreto s/controlador','Sistema Discreto c/controlador');

%% Acción de control
Gu=minreal(feedback(Gcd,Gpda));
figure; step(Gu)
legend('Acción de Control')
%% ***************************************************
%% FT deseada
Gdes=1/((z - pz1)*(z - pz2));
figure; pzplot(Gpda); hold on
pzplot(Gdes);
pzplot(Gcd);
legend('Polos-ceros FT LA SC','Polos deseados pz','Polo-Cero Gcd')

%% ***************************************************
figure; margin(Glad); grid
legend('Estabilidad del Sistema Compensado');

%% ***************************************************
%% Error de Velocidad
t=0:T:10;
rampa=t;
Gesc=minreal(feedback(1,Gpda));
Gecc=minreal(feedback(1,Gcd*Gpda));
figure; lsim(Gesc,Gecc,rampa,t)
legend('essv sistema sin controlador','essv sistema con controlador')

% Kv=minreal((1/T)*(0.10979*(1+0.8906)*(z-1))/((1-0.07246)*(z-1)));
% essv=100*(1/Kv);