clear
close all

s=tf('s');

Vrms=220;
Vpico=Vrms*sqrt(2);         % Valor de pico maximo de la tension de salida
Pnom=1000;                  % Potencia activa nominal de salida
Irms=Pnom/Vrms;             % Valor eficaz de la corriente de salida

% Planta: Filtro pasa bajos LC a la salida de un inversor PWM monofasico
% Parametros. Filtro LC y Carga
L=500e-6;            % Indutancia en uH
C=60e-6;             % Capacitancia en uF
wo=1/sqrt(L*C);      % Frecuencia angular de resonancia del filtro LC en rad/seg
fo=wo/2/pi;          % Frecuencia de resonancia del filtro LC en Hz
R=Vrms/Irms;        % Resistencia de carga para la potencia nominal en Ohms 

% Funcion de transferencia del filtro LC
Gp=(1/L/C)/(s^2 + (1/R/C)*s + (1/L/C));
Gplc=feedback(Gp,1);
figure; step(Gplc)
figure; pzmap(Gplc)  % xita=2% y Mp aproximadamente 94%

%% ----------------------------------------------------------------------
fm=8000;   % frecuencia de muestreo en Hz
T=1/fm;    % periodo de muestreo en segundos

z=tf('z',T);

% Funcion de trasnferencia muestreada de la planta
Gpd=c2d(Gp,T);
% Gpdlc=feedback(Gpd,1);
% figure; step(Gpdlc)   % planta en LC sin compensador: inestable
% figure; pzmap(Gpdlc)

% Especificaciones de proyecto
Mp=2/100;                % Sobrepaso maximo del 5%. Mp=exp(-pi.sigma/wd)
ts=2e-3;                 % Tiempo de establecimiento 1 mseg. ts=4/sigma
sigma=4/ts;              % Parte real de los polos dominantes de lazo cerrado en el plano-s
wd=-pi*sigma/log(Mp);    % Parte imaginaria de los polos dominantes de lazo cerrado en el plano-s
s1=-sigma+1i*wd;         % Polos dominantes de lazo cerrado en el plano-s
s2=-sigma-1i*wd;

z1=exp(s1*T);            % Polos dominantes de lazo cerrado en el plano-z
z2=exp(s2*T);

figure; pzmap(Gpd)
hold on; plot(real(z1),imag(z1),'x');

close all

[Z,P,Kplanta]=zpkdata(Gpd,'v');

% Polos y ceros de la planta discreta y polos y ceros del compensador;
pp1=P(1,1);
pp2=P(2,1);
zz1=Z(1,1);

den_comp_PD_pred=z^2;   % Dos polos al origen del PD predictivo
Pcomp=zero(den_comp_PD_pred); 
pc1=Pcomp(1);
pc2=Pcomp(2);

% Proyecto Compensador PD predictivo discreto
% Condicion de angulo

% Vectores de los polos de la planta y el compensador a z1
v1=(real(pp1)-real(z1)) + 1i*(imag(pp1)-imag(z1));
v2=(real(pp1)-real(z1)) + 1i*(imag(pp1)+imag(z1));
v3=z1 - pc1;
v4=z1 - pc2;

% Vectores de los ceros de la planta y el compensador a z1
v5=(real(z1)+abs(zz1)) + 1i*imag(z1);
% v6=(real(z1)+abs(zc)) + 1i*imag(z1);

% Angulos de los polos a z1
fi1=2*pi + angle(z1-pp1); fi1g=fi1*180/pi;
fi2=angle(z1-pp2); fi2g=fi2*180/pi;
fi3=angle(v3); fi3g=fi3*180/pi;
fi4=angle(v4); fi4g=fi4*180/pi;

% Angulos de los ceros a z1
tita1=angle(v5); tita1g=tita1*180/pi;
% titazc=angle(v6); 

% Calculo la deficiencia de fase en grados sexagecimales de la planta
suma = tita1g - fi1g - fi2g - fi3g - fi4g;

deficiencia_g180 = suma + 180;
% Como la deficiencia es mayor a -180°, entonces la condición de fase debe ser igual a 360°
deficiencia_g360 = suma + 360;

% Angulo del vector del cero del compensador al punto z1: Condicion de fase
% igual a 0° o 360°

titazc=-deficiencia_g360;

% Ubicacion del cero del compensador: Si da positivo es porque zc esta del
% lado izquierdo del plano-z (entre 0 y -1). Caso contrario, zc esta del
% lado derecho del plano-z (entre 0 y 1).
% zc = (imag(z1)/tand(titazc)) - real(z1);

zc = real(z1) - (imag(z1)/tand(titazc));

% Vector del cero del compensador a z1
v6=(real(z1)-abs(zc)) + 1i*imag(z1);

% Condicion de modulo
K1=-(abs(v1)*abs(v2)*abs(v3)*abs(v4))/(Kplanta*abs(v5)*abs(v6));

Gcd=K1*(z - zc)/z^2;

% FT lazo abierto
Glad=Gpd*Gcd;
% FT lazo cerrado
Glcd=feedback(Glad,1);
figure; pzmap(Glcd)

figure; step(Glcd);
legend('sistema compensado');

% FT con accion feedforward
Glcfeed = Gpd*(1 + Gcd)/(1 + Gpd*Gcd);

figure; step(Glcfeed);
legend('sistema compensado con feedforward');

%% --- Con 5 kHz de frecuencia de muestreo no se consigue obtener el desempeño buscado 
%  por lo tanto de realiza un ajuste con SISOTOOL

% zc_siso=-0.2143;
% K1_siso=-0.67401;
% 
% Gcd_adj = K1_siso*(z + zc_siso)/z^2;
% 
% % FT lazo abierto
% Glad_adj=Gpd*Gcd_adj;
% % FT lazo cerrado
% Glcd_adj=feedback(Glad_adj,1);
% figure; pzmap(Glcd_adj)
% 
% figure; step(Glcd_adj);
% legend('sistema compensado');
% 
% % Funcion de Transferencia con accion feedforward
% Glcfeed_adj = Gpd*(1 + Gcd_adj)/(1 + Gpd*Gcd_adj);
% 
% figure; step(Glcfeed_adj);
% legend('sistema compensado con feedforward');