% DEFINICIÓN DE PARAMETROS DE SIMULACIÓN
clc; clear
close all
s=tf('s');
format long

fr=50;					% FRECUENCIA DESEADA DE LA TENSION DE SALIDA EN Hz
wo=2*pi*fr;             % FRECUENCIA ANGULAR DESEADA DE LA TENSION DE SALIDA EN rad/seg
mf=200;          		% NUMERO DE PULSOS PWM POR PERIODO
fs=mf*fr;               % FRECUENCIA DE CONMUTACION
Ts=1/fs;				% PERIODO DE COMUTACION
fm=fs;                  % FRECUENCIA DE MUESTREO
Tm=1/fm;                % PERIODO DE MUESTREO.
frep=fs;                % Frequencia de muestreo y frecuencia de actualización acción MI
Tr=1/frep;              % PERIODO actualización de la acción del Modelo Interno
ms=round(fm/fr);        % NUMERO DE MUESTRAS POR PERIODO DE LA TENSION DE SALIDA
mr=round(frep/fr);      % NUMERO DE MUESTRAS DEL CONTROLADOR POR MODELO INTERNO EN UN PERIODO FUNDAMENTAL
nc=20;				    % NUMERO DE PERIODOS DE SIMULACION
na=nc*ms;               % NUMERO DE TOTAL DE MUESTRAS POR SIMULACIÓN
p=2000;                 % NUMERO DE PUNTOS POR MUESTRA (PASO DE SIMULACION)
np=na*p;                % NUMERO TOTAL DE PUNTOS DE SIMULACIÓN
nr=mr*nc;               % NUMERO TOTAL DE MUESTRAS DEL CONTROLADOR POR MI
Vbase=311;              % VALOR BASE DE TENSIÓN PARA NORMALIZACIÓN
Ibase=50;               % VALOR BASE DE CORRIENTE PARA NORMALIZACIÓN
Vcc=450;
N=mr;                   % NUMERO DE MUESTRAS DEL CONTROLADOR POR MI

z=tf('z',Tm);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETROS DE LA PLANTA. Filtro y Carga
Lf=250e-6;              % Inductancia de entrada en uH
Cf=60e-6;          	    % Capacitancia del filtro de salida en uF
rl=0.188;               % Resistencia serie del inductor en Ohms
wn=1/sqrt(Lf*Cf);       % Frequencia angular de resonancia del filtro LC en rad/seg
fn=wn/2/pi;             % Frequencia de resonancia del filtro LC en Hz
Rc=1.5;                   % Resistencia de carga 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parametros del rectificador utilizado como carga no lineal
Cdc=1000e-6;
Rdc=30;
Rs=0.5;
% Matrices de simulación del inversor con carga: 
% Vector de estado: x = [iL vo]'
Carga=-1/Rc/Cf;
 
A=[-rl/Lf  -1/Lf
    1/Cf     Carga  ];
     
B=[Vcc/Lf;0];

F=[0;-1/Cf];

C=[0 1];

D=0;

sys_ee=ss(A,B,C,D);  % Define un sistema en el espacio de estado
figure; step(sys_ee); hold on

% Funcion de transferencia de la planta en tiempo continuo
[num_Gp,den_Gp] = ss2tf(A,B,C,D,1);

Gp = tf(num_Gp,den_Gp);  % FT para la matriz de salida elegida
step(Gp)
legend('Rta EE','Rta Gp')

val_car=eig(A)     % Obtiene los autovalores de la matriz A
polos_GP=pole(Gp)  % Obtiene los polos de la FT Gp

%% EE a partir de Gp(s)
[A1,B1,C1,D1]=tf2ss(num_Gp,den_Gp);
sys_ee1=ss(A1,B1,C1,D1);
figure; step(sys_ee1)

% Funcion de transferencia de la planta en tiempo discreto con atraso de la implementación digital
Gpd= (1/z)*c2d(Gp,Tm,'zoh');
figure; rlocus(-1*Gpd)
% Análisis de estabilidad de la planta con compensador convencional
Kp=-0.2;
Gc=Kp;
Gla_P = series(Gc,Gpd);
figure; rlocus(Gla_P)
figure; bode(Gla_P); grid
Glc_P = feedback(Gla_P,1);
% figure; pzmap(Glc_PD)
figure; bode(Glc_P)
figure; step(Glc_P)
close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Proyecto controladores resonantes
% Resonante en la frecuencia de la fundamental
Gr1 = s/(s^2 + wo^2);
% Resonantes en las armonicas de la fundamental hasta 11ra
Gr3 = s/(s^2 + (3*wo)^2);
Gr5 = s/(s^2 + (5*wo)^2);
Gr7 = s/(s^2 + (7*wo)^2);
Gr9 = s/(s^2 + (9*wo)^2);
Gr11= s/(s^2 + (11*wo)^2);

% Controladores resonantes en tiempo discreto
Grd1=c2d(Gr1,Tm,'zoh');      %50 Hz        
Grd3=c2d(Gr3,Tm,'zoh');      %150 Hz  
Grd5=c2d(Gr5,Tm,'zoh');      %250 Hz
Grd7=c2d(Gr7,Tm,'zoh');      %350 Hz
Grd9=c2d(Gr9,Tm,'zoh');      %450 Hz
Grd11=c2d(Gr11,Tm,'zoh');    %550 Hz

% Ganancias controladores resonantes
kmi_arm = 20;          %10
kmi_1 = 400;          %50 Hz
kmi_3 = kmi_arm;      %150 Hz
kmi_5 = kmi_arm;      %250 Hz    
kmi_7 = kmi_arm;      %350 Hz  
kmi_9 = kmi_arm;      %450 Hz
kmi_11= kmi_arm;      %550 Hz

Gmi1 = kmi_1*Grd1;
Gmi3 = kmi_3*Grd3;
Gmi5 = kmi_5*Grd5;
Gmi7 = kmi_7*Grd7;
Gmi9 = kmi_9*Grd9;
Gmi11 = kmi_11*Grd11;

%% Analisis de desempeño y estabilidad
P=bodeoptions;
P.FreqUnits = 'Hz'; % Grafica diagramas de Bode con frecuencia en Hz

% Funcion de transferencia de lazo abierto: Gcc + Gpd
Gcc=Gc + Gmi1; % Solo el compensador resonante fundamental
Gla = series(Gpd,Gcc);
figure; margin(Gla)
figure; pzmap(Gla);

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

periodos=nc/fr;
t=0:Tm:periodos-Tm;
r=sin(2*pi*fr*t);
figure;
lsim(Glc,r,t);
close all    

 % Resonante de la Fundamental + resonantes de los armónicos
Gcc=Gc + Gmi1 + Gmi3 + Gmi5 + Gmi7 + Gmi9 + Gmi11;
Gla_PMI = series(Gcc,Gpd);
figure; margin(Gla_PMI)
figure; pzmap(Gla_PMI);

Glc_PMI = feedback(Gla_PMI,1);
figure; pzmap(Glc_PMI)
figure; bode(Glc_PMI,P)

figure; lsim(Glc_PMI,r,t);
vo=lsim(Glc_PMI,r,t);
ev=r' - vo;
figure(11); plot(t,ev)
close all

%% Matriz de Normalización  
Tn=[1/Ibase    0
       0    1/Vbase];
% Matrices de la Planta Normalizadas   
An=Tn*A*inv(Tn);
Bn=Tn*B*Vbase;
Fn=Tn*F*Ibase;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Discretización de la planta y matrices de estado con atraso de implementación
[G,H]=c2d(An,Bn,Tm);

Td=Tm;
[eAtD,H1]=c2d(An,Bn,Td);                      
[eATtD,H2]=c2d(An,Bn,Tm-Td);                  

% Matriz H0
Ho=eATtD*inv(An)*(eAtD-eye(2))*Bn;          

% Matriz H1 
H1=inv(An)*(eATtD-eye(2))*Bn;

% Matrices Gp y Hp de la planta en tiempo discreto que modelan el atraso de implementación digital
Gp=[G Ho;zeros(1,3)];					              
Hp=[H1;1];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Discretizacion de las matrices de estado para simulacion en tiempo
% continuo con un periodo de muestreo Tm/p
[Gc,Hc]=c2d(A,B,Tm/p);
[Gcc,Fc]=c2d(A,F,Tm/p);

%% Coeficientes de los controladores resonantes
[num,den]=tfdata(Gmi1,'v');
a2_1 = num(1);
a1_1 = num(2);
a0_1 = num(3);
b1_1 = den(2);
b0_1 = den(3);

[num,den]=tfdata(Gmi3,'v');
a2_3 = num(1);
a1_3 = num(2);
a0_3 = num(3);
b1_3 = den(2);
b0_3 = den(3);
   
[num,den]=tfdata(Gmi5,'v');
a2_5 = num(1);
a1_5 = num(2);
a0_5 = num(3);
b1_5 = den(2);
b0_5 = den(3);   
   
[num,den]=tfdata(Gmi7,'v');
a2_7 = num(1);
a1_7 = num(2);
a0_7 = num(3);
b1_7 = den(2);
b0_7 = den(3);   
   
[num,den]=tfdata(Gmi9,'v');
a2_9 = num(1);
a1_9 = num(2);
a0_9 = num(3);
b1_9 = den(2);
b0_9 = den(3);   
   
[num,den]=tfdata(Gmi11,'v');
a2_11 = num(1);
a1_11 = num(2);
a0_11 = num(3);
b1_11 = den(2);
b0_11 = den(3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inicialización de las variables utilizadas en la simulación
kc=1:na*p;    % Magnitud de los vectores de las variables en tiempo continuo con periodo de muestreo Tm/p
ka=1:na;      % Magnitud de los vectores de las variables en tiempo continuo con periodo de muestreo Tm

% Vectores de la planta en tiempo discreto
Xm=zeros(2,length(ka));
xe=zeros(3,length(ka));
vc=zeros(1,length(ka));
ure=zeros(1,length(ka));
up=zeros(1,length(ka));
upd=zeros(1,length(ka));
umi=zeros(1,length(ka));
umi_1=zeros(1,length(ka));
umi_3=zeros(1,length(ka));
umi_5=zeros(1,length(ka));
umi_7=zeros(1,length(ka));
umi_9=zeros(1,length(ka));
umi_11=zeros(1,length(ka));
umi_13=zeros(1,length(ka));
umi_15=zeros(1,length(ka));
umi_17=zeros(1,length(ka));
umi_19=zeros(1,length(ka));
u=zeros(1,length(ka));
uc=zeros(1,length(ka));
e=zeros(1,length(ka));
ei=zeros(1,length(ka));
y=zeros(1,length(ka));
r=zeros(1,length(ka));
td=zeros(1,length(ka));
iL=zeros(1,length(ka));
urp=zeros(1,length(ka));
Se=zeros(1,length(ka));
Id=zeros(1,length(ka));
Id1=zeros(1,length(ka));
uffc=zeros(1,length(ka));

% Vectores de la planta en tiempo continuo
x=zeros(2,length(kc));
Io=zeros(1,length(kc));
Ior=zeros(1,length(kc));
upwm=zeros(length(kc),1);
PWM1=zeros(length(kc),1);
PWM2=zeros(length(kc),1);
io=zeros(length(kc),1);
t=zeros(length(kc),1);
portadora=zeros(length(kc),1);
Vdc=zeros(length(kc),1);
Vdc(1:100,1)=278*ones(100,1);
vo=zeros(1,length(kc));

Timer1=0;
flag=1;
sinal_port=1;
ComparReg1=p/2;
ComparReg2=p/2;
km=3;

% Simula el inversor con carga lineal o no lineal
% Load = 1 (carga lineal) Load = 0 (carga no lineal) 
Load=0;
Perturba=0;

% Flags de decisión
% Con Load=1 para carga lineal:
    % 1er caso a probar: armonicas=0; Prop=0; y SR=1;
    % 2do caso a probar: armonicas=1; Prop=0; y SR=1;
    % 3er caso a probar: armonicas=0; Prop=1; y SR=0;
% Con Load=0 para carga no lineal:
    % 1er caso a probar: armonicas=0; Prop=0; y SR=1;
    % 2do caso a probar: armonicas=1; Prop=0; y SR=1;
    % 3er caso a probar: armonicas=1; Prop=1; y SR=0;
% TODOS LOS CASOS CONTEMPLADOS FUNCIONAN SIN DESESTABILIZAR AL INVERSOR
LA=0;         % Operación a lazo abierto: 1 opera a lazo abierto
armonicas=1;  % Suma al resonante de la fundamental, los resonantes de las armónicas
Prop=0;       % Suma el compensador proporcional a los resonantes
SR=1;         % La acción de control total es la que resulta de los resonantes

fn=(Vbase/Vcc);

% Inicia Barra de Progresión
perc=0.01;
passo=0.01;
wb = waitbar(0,'Aguarde, Simulando Inversor');

% Simulação
for k=2:np
    t(k)=k*Tm/p;
    Timer1=Timer1 + sinal_port;

    % Muestreo y cálculo de la acción de control
    if flag==1
        flag=0;
        km=km+1;
        td(km)=km*Tm;
        
        % Muestreo de la corriente iL y de la tensión vc
        Xm(:,km)=[x(1,k);x(2,k)];
        % Normalización de variables
        iL(km)=Xm(1,km)/Ibase;
        vc(km)=Xm(2,km)/Vbase;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Referencia de Tensión
        r(km)=sin(2*pi*fr*km*Tm);

        % Error de Tensión
        e(km)=r(km) - vc(km);

        % Lazo de tensión
        % Calculo del controlador resonante en la fundamental + armonicas
        % 1ra, 3ra, 5ta, 7ma, 9na y 11ra 
        umi_1(km) = a2_1*e(km) + a1_1*e(km-1) + a0_1*e(km-2) - b1_1*umi_1(km-1) - b0_1*umi_1(km-2); 
        umi_3(km) = a2_3*e(km) + a1_3*e(km-1) + a0_3*e(km-2) - b1_3*umi_3(km-1) - b0_3*umi_3(km-2);
        umi_5(km) = a2_5*e(km) + a1_5*e(km-1) + a0_5*e(km-2) - b1_5*umi_5(km-1) - b0_5*umi_5(km-2);
        umi_7(km) = a2_7*e(km) + a1_7*e(km-1) + a0_7*e(km-2) - b1_7*umi_7(km-1) - b0_7*umi_7(km-2);
        umi_9(km) = a2_9*e(km) + a1_9*e(km-1) + a0_9*e(km-2) - b1_9*umi_9(km-1) - b0_9*umi_9(km-2);
        umi_11(km)= a2_11*e(km) + a1_11*e(km-1) + a0_11*e(km-2) - b1_11*umi_11(km-1) - b0_11*umi_11(km-2);
        
        if armonicas == 1
            umi(km)=umi_1(km) + umi_3(km) + umi_5(km) + umi_7(km) + umi_9(km) + umi_11(km);
        else
            umi(km)=umi_1(km);
        end

        % Acción de control con modelo interno + proporcional:
        if Prop == 1
            up(km) = Kp*e(km);  % Compensador Proporcional
            u(km) = umi(km) + up(km);
        end

        if SR == 1
            u(km) = umi(km);
        end
        
        if LA == 1
            u(km)=r(km);
        end
        uc(km)=u(km)*(p/4)*fn;
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if Timer1==p/2
        sinal_port=-1;
        flag=1;
    end

    if Timer1<0
        sinal_port=1;
    end

    % Actualización de los comparadores
    if Timer1==0
        ComparReg1= uc(km)+(p/4);
        ComparReg2=-uc(km)+(p/4);
    end
    
    if ComparReg1>Timer1
        PWM1(k)=1;
    else
        PWM1(k)=0;
    end
    
    if ComparReg2>Timer1
        PWM2(k)=1;
    else
        PWM2(k)=0;
    end
    
    upwm(k)=(PWM1(k)-PWM2(k))*Vcc;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if Load == 1
        Ior(k)=x(2,k)/Rc;
    end
    
    if Load == 1 && Perturba == 1
        Ior(k)=x(2,k)/Rc/10;
        if k>=(0.6*np)+(ms*p/4)
            Ior(k)=x(2,k)/Rc;
        end
        if k>=0.85*np
            Ior(k)=x(2,k)/Rc;
        end
    end
    
    % Retificador no controlado con filtro capacitivo
    if abs(x(2,k))>=Vdc(k)
        Io(k)=sign(x(2,k))*(abs(x(2,k))-Vdc(k))/Rs;
        Vdc(k+1)= Vdc(k) + Tm/p/Cdc*(( abs(x(2,k)) - Vdc(k) )/Rs - Vdc(k)/Rdc );
    else
        Io(k)=0;
        Vdc(k+1)= Vdc(k) + Tm/p/Cdc*(-Vdc(k)/Rdc );
    end
    
%     Io(k)=Ior(k);
    
    vo(k)=x(2,k);
    
    x(:,k+1)= Gc*x(:,k) + Hc*upwm(k) + Fc*Io(k);
    
    % Atualiza a barra de progressão
    if fix((round(np))*perc)==k
        waitbar(perc)
        perc=perc+passo;
    end
end
close(wb)

figure
plot(td(1:km),e(1:km))
legend('error')
axis([0 max(td) -1.0 1.0])

if Prop == 1
    figure
    plot(td(1:km),u(1:km),td(1:km),umi(1:km),td(1:km),up(1:km));
    legend('Accion de control total','Accion de control resonante','Accion de control proporcional')
end

if SR == 1
    figure
    plot(td(1:km),u(1:km),td(1:km),umi(1:km));
    legend('Accion de control total','Accion de control resonante')
end

figure
plot(td(1:km),vc(1:km),'r')
hold on
plot(td(1:km),r(1:km),'k')
grid; axis([(max(td)-(0.04)) max(td) -1.2 1.2])
legend('vc','ref')

figure
plot(t,x(2,1:k),'r')
hold
plot(t,Io)
grid; 
% axis([(max(t)-(2/fr)) max(t) -400 400])
legend('vc','io')

THD_tension_salida
% close all
%% Genera archivo ganancias.h para el Dev C++
fid = fopen('ganancias_inversor.h','w');

fprintf(fid, '#define fsm %d\n', fs);

fprintf(fid, '#define a2_1 %1.12f\n', a2_1);
fprintf(fid, '#define a1_1 %1.12f\n', a1_1);
fprintf(fid, '#define a0_1 %1.12f\n', a0_1);
fprintf(fid, '#define b1_1 %1.12f\n', b1_1);
fprintf(fid, '#define b0_1 %1.12f\n', b0_1);

fprintf(fid, '#define a2_3 %1.12f\n', a2_3);
fprintf(fid, '#define a1_3 %1.12f\n', a1_3);
fprintf(fid, '#define a0_3 %1.12f\n', a0_3);
fprintf(fid, '#define b1_3 %1.12f\n', b1_3);
fprintf(fid, '#define b0_3 %1.12f\n', b0_3);

fprintf(fid, '#define a2_5 %1.12f\n', a2_5);
fprintf(fid, '#define a1_5 %1.12f\n', a1_5);
fprintf(fid, '#define a0_5 %1.12f\n', a0_5);
fprintf(fid, '#define b1_5 %1.12f\n', b1_5);
fprintf(fid, '#define b0_5 %1.12f\n', b0_5);

fprintf(fid, '#define a2_7 %1.12f\n', a2_7);
fprintf(fid, '#define a1_7 %1.12f\n', a1_7);
fprintf(fid, '#define a0_7 %1.12f\n', a0_7);
fprintf(fid, '#define b1_7 %1.12f\n', b1_7);
fprintf(fid, '#define b0_7 %1.12f\n', b0_7);

fprintf(fid, '#define a2_9 %1.12f\n', a2_9);
fprintf(fid, '#define a1_9 %1.12f\n', a1_9);
fprintf(fid, '#define a0_9 %1.12f\n', a0_9);
fprintf(fid, '#define b1_9 %1.12f\n', b1_9);
fprintf(fid, '#define b0_9 %1.12f\n', b0_9);

fprintf(fid, '#define a2_11 %1.12f\n', a2_11);
fprintf(fid, '#define a1_11 %1.12f\n', a1_11);
fprintf(fid, '#define a0_11 %1.12f\n', a0_11);
fprintf(fid, '#define b1_11 %1.12f\n', b1_11);
fprintf(fid, '#define b0_11 %1.12f\n', b0_11);

fprintf(fid, '#define Kp %1.12f\n',Kp);

fclose(fid)