% DEFINICIÓN DE PARAMETROS DE SIMULACIÓN
clear
close all
s=tf('s');
P=bodeoptions;
P.FreqUnits = 'Hz';     % Grafica diagramas de Bode con frecuencia en Hz

fr=50;					% FRECUENCIA DE LA COMPONENTE FUNDAMENTAL DE LA REFERENCIA EN Hz
wo=2*pi*fr;             % FRECUENCIA ANGULAR DE RESONANCIA EN rad/seg
nc=20;                  % PERIODOS DE SIMULACIÓN PARA REFERENCIA SINUSOIDAL

fm=10000;
Tm=1/fm;
z=tf('z',Tm);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETROS DE LA PLANTA. Filtro y Carga
Lf=250e-6;              % Indutancia de entrada en uH
Cf=40e-6;          	    % Capacitancia del filtro de salida en uF
Rl=0.188;               % Resistencia serie del inductor en Ohm
wn=1/sqrt(Lf*Cf);       % Frecuencia angular de resonancia del filtro LC en rad/seg
fn=wn/2/pi;             % Frecuencia de resonancia del filtro LC en Hz
Rc=10;                  % Resistencia de carga 
Vcc=180;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Matrices del modelo en el espacio de estados 
% Vector de estado: x = [iL vc]' 
% iL: Corriente en el inductor del filtro
% vc: Tensión en bornes del capacitor del filtro. Igual a la tensión en bornes de la carga
Carga=-1/Rc/Cf;
 
A=[-Rl/Lf  -1/Lf
    1/Cf    Carga];
     
B=[1/(Lf);0];

F=[0;-1/Cf];

C=[0 1];

D=0;

% Función de transferencia de la planta en tiempo continuo
[num_Gp,den_Gp] = ss2tf(A,B,C,D);
Gp = zpk(tf(num_Gp,den_Gp));

% Funcion de transferencia de la planta en tiempo discreto
Gpd = (1/z)*c2d(Gp,Tm,'zoh');
figure; rlocus(-1*Gpd)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Diseño controladores resonantes
% Resonante en la frecuencia de la fundamental
Gr1 = s/(s^2 + wo^2);

% Resonantes en las armonicas de la fundamental
Gr3 = s/(s^2 + (3*wo)^2);
Gr5 = s/(s^2 + (5*wo)^2);
Gr7 = s/(s^2 + (7*wo)^2);

% Controladores resonantes en tiempo discreto: ZOH
Grd1_zoh=c2d(Gr1,Tm,'zoh');      %50 Hz        
Grd3_zoh=c2d(Gr3,Tm,'zoh');      %150 Hz  
Grd5_zoh=c2d(Gr5,Tm,'zoh');      %250 Hz
Grd7_zoh=c2d(Gr7,Tm,'zoh');      %350 Hz

% Controladores resonantes en tiempo discreto: FOH
Grd1_foh=c2d(Gr1,Tm,'foh');      %50 Hz        
Grd3_foh=c2d(Gr3,Tm,'foh');      %150 Hz  
Grd5_foh=c2d(Gr5,Tm,'foh');      %250 Hz
Grd7_foh=c2d(Gr7,Tm,'foh');      %350 Hz

% Controladores resonantes en tiempo discreto: IMPULSE-INVARIANT
Grd1_imp=c2d(Gr1,Tm,'impulse');      %50 Hz        
Grd3_imp=c2d(Gr3,Tm,'impulse');      %150 Hz  
Grd5_imp=c2d(Gr5,Tm,'impulse');      %250 Hz
Grd7_imp=c2d(Gr7,Tm,'impulse');      %350 Hz

% Aproximaciones en cada caso vistas en clase
Kr_zoh=sin(wo*Tm)/wo; a1=2*cos(wo*Tm);
Kr_foh=(1 - cos(wo*Tm))/(Tm*wo^2);
Kr_imp=Tm; b1=cos(wo*Tm);

Gr_zoh=minreal((Kr_zoh*(z^-1 - z^-2))/(1 - a1*z^-1 + z^-2));
Gr_foh=minreal((Kr_foh*(1 - z^-2))/(1 - a1*z^-1 + z^-2));
Gr_imp=minreal((Kr_imp*(1 - b1*z^-1))/(1 - a1*z^-1 + z^-2));

% Graficas de Bode
% Comparación respuestas en frecuencia de Gr1(s) con las 3 aproximaciones
figure(1); bode(Gr1,Gr_zoh,Gr_foh,Gr_imp,P)
legend('Gr1','Gr_{zoh}','Gr_{foh}','Gr_{imp}')

% Comparación de Gr1(s) con la aproximación por ZOH 
figure(2); bode(Gr1,Gr_zoh,P)
% Compensación polo-cero del ZOH
GC=Gr_zoh*(2*(z-0.009))/(z+1-(2*0.009));
figure(3); bode(Gr1,GC,P)
% Comparación de Gr1(s) con la aproximación por FOH
figure(4); bode(Gr1,Gr_foh,P)
% Comparación de Gr1(s) con la aproximación Invariante al Impulso
figure(5); bode(Gr1,Gr_imp,P)

close all
kmi = 100;
% Análisis de estabilidad con la planta del inversor para las 3 aproximaciones: solo fundamental
figure; bode(series(kmi*Gr_zoh,Gpd),series(kmi*Gr_foh,Gpd),series(kmi*Gr_imp,Gpd),P)
legend('LA con Gr_{zoh}','LA con Gr_{foh}','LA con Gr_{imp}')

% Ganancias para los controladores resonantes
kmi_1 = 450;         %50 Hz 450
kmi_3 = 50;          %150 Hz
kmi_5 = 50;          %250 Hz    
kmi_7 = 50;          %350 Hz  

Gmi1 = kmi_1*Grd1_zoh;
Gmi3 = kmi_3*Grd3_zoh;
Gmi5 = kmi_5*Grd5_zoh;
Gmi7 = kmi_7*Grd7_zoh;

%% Analisis de desempeño y estabilidad: controlador resonante fundamental

% Funcion de transferencia de lazo abierto: Gc + Gpd
Kp=0.1;
Gc=Gmi1; % + Kp;

Gla = series(Gpd,Gc);
figure(6); margin(Gla)
figure(7); pzmap(Gla);

% Funcion de trasnferencia de LC con resonante fundamental
Glc = feedback(Gla,1);
figure(8); bode(Glc,P) 
% figure(9); pzmap(Glc);

A=1;
periodos=nc/fr;
t=0:Tm:periodos-Tm;
r=A*sin(2*pi*fr*t);
figure(10); lsim(Glc,r,t);
vo=lsim(Glc,r,t);
ev=r' - vo;
figure(11); plot(t,ev)
Gumi=feedback(Gc,Gpd);
figure(12); lsim(Gumi,ev,t)
close all

%% Análisis con fundamental y resonantes
% Funcion de transferencia de lazo abierto: Gc + Gpd
Kp=0.1;
Gc=Gmi1+Gmi3+Gmi5+Gmi7; %+Kp;

Gla = series(Gpd,Gc);
figure(6); margin(Gla)
figure(7); pzmap(Gla);

% Funcion de trasnferencia de LC con resonante fundamental
Glc = feedback(Gla,1);
figure(8); bode(Glc,P)
figure(9); pzmap(Glc);

A=1;
periodos=nc/fr;
t=0:Tm:periodos-Tm;
r=A*sin(2*pi*fr*t);
figure(10); lsim(Glc,r,t);
vo=lsim(Glc,r,t);
ev=r' - vo;
figure(11); plot(t,ev)
Gumi=feedback(Gc,Gpd);
figure(12); lsim(Gumi,ev,t)