% Este script fue desarrollado por el Ing. Botteron y modificado por Roberto
% Carballo, con el fin de implementar un controlador por modelo interno en
% el simulador PSIM, estudiando y desarrollando los compensadores de modelo
% interno y clasicos en este script.

% DEFINICION DE PARAMETROS DE SIMULACION
clear
close all
 
fr=50;                 % FRECUENCIA DESEADA DE LA TENSION DE SALIDA EN Hz
Tr=1/fr;               % PERIODO DE LA COMPONENTE FUNDAMENTAL EN segundos
w=round(2*pi*fr);      % FRECUENCIA ANGULAR DESEADA DE LA TENSION DE SALIDA EN rad/seg
fsw=10000;             % FRECUENCIA DE CONMUTACION EN Hz
mf=fsw/fr;             % NUMERO DE PULSOS PWM POR PERIODO: 5KHz/50Hz = cantidad de puntos de tabla 100
Tsw=1/fsw;             % PERIODO DE CONMUTACION EN segundos
fs=fsw;                % FRECUENCIA DE MUESTREO EN Hz
Ts=1/fs;               % PERIODO DE MUESTREO EN segundos
fmi=fs;                % FRECUENCIA DE MUESTREO DEL MODELO INTERNO = frecuencia de muestreo
Tmi=1/fmi;             % PERIODO DE MUESTREO DEL MODELO INTERNO 
ms=round(fs/fr);       % NUMERO DE MUESTRAS POR PERIODO DE LA TENSION DE SALIDA
N=round(fmi/fr);       % NUMERO DE MUESTRAS DEL MODELO INTERNO EN UN PERIODO DE LA TENSION DE SALIDA,

mi_aimp=1;             % Flag para modelo interno de armonicas impares
mi_apim=0;             % Flag para modelo interno de armonicas pares e impares

% Valores base para normalización de la planta a valores en p.u.
Vrms=220;
Vbase=Vrms*sqrt(2);         % VALOR BASE DE TENSION PARA NORMALIZACION (VALOR DE PICO MAXIMO)
Pbase=1e3;                  % POTENCIA BASE
Ibase=Pbase*sqrt(2)/Vrms;   % VALOR BASE DE CORRIENTE PARA NORMALIZACION (VALOR DE PICO MAXIMO)
Vcc=350;                    % VALOR DE LA FUENTE DE TENSION CC DE ENTRADA
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETROS DE LA PLANTA. Filtro LC y Carga
Lf=500e-6;            % Indutancia en uH
Cf=60e-6;             % Capacitancia en uF
wo=1/sqrt(Lf*Cf);     % Frecuencia angular de resonancia del filtro LC en rad/seg
fo=wo/2/pi;           % Frecuencia de resonancia del filtro LC en Hz
Rc=Vbase/Ibase;       % Resistencia de carga para la potencia nominal en Ohms 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matrices de estado de la planta
% Vector de estados: x=[iL;vc] 
A=[ 0    -1/Lf
   1/Cf    0  ];

B=[1/Lf;0];

F=[0;-1/Cf];

C=[0 1];

D=0;

Tn=[1/Ibase     0    
     0        1/Vbase];

An=Tn*A/Tn;
Bn=Tn*B*Vbase;

Co=ctrb(An,Bn);

% Number of uncontrollable states
unco_sc=length(An)-rank(Co); % Ahora el modelo es controlable

%% --------------------------------------------------------
% Discretizacion del sistema
% Obtención de las matrices de la planta discreta considerando el atraso de transporte
% de la implementacion digital en tiempo real
Td=Ts;   % Se considera un tiempo de atraso igual al periodo de discretizacion Ts
 
[G,H]=c2d(An,Bn,Ts);
 
[eAtD,H11]=c2d(An,Bn,Td);                      
[eATtD,H2]=c2d(An,Bn,Ts-Td);

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

% Matrices Gp y Hp de la planta discreta que modelan el atraso Td
Gp=[G Ho;zeros(1,3)];					              
Hp=[H1;eye(1,1)];
Cp=[C zeros(1,1)];
Dp=0;

Co=ctrb(Gp,Hp);
% Numero de estados no controlables
unco_sd=length(Gp)-rank(Co);

sys_dsc=ss(Gp,Hp,Cp,Dp,Ts);
figure; pzmap(sys_dsc); axis([-1 1 -1 1])
figure; margin(sys_dsc) 

[num1,den1]=ss2tf(Gp,Hp,Cp,Dp,1);

Gz_11=tf(num1,den1,Ts);

%% -----------------------------------------

% s=tf('s');
% Gp=(1/Lf/Cf)/(s^2 + (1/Rc/Cf)*s + (1/Lf/Cf));
% Gpd=c2d(Gp,Ts);

Gpd=minreal(Gz_11);
[numd,dend]=tfdata(Gpd,'v');
[Z,P,Kplanta]=zpkdata(Gpd,'v');

% Especificaciones de desempeño
Mp=5/100; ts=1e-3; 

sigma=4/ts;
wd=-sigma*pi/log(Mp);
wn=(sigma^2+wd^2)^(1/2);
zeta=sigma/wn;

% Polo deseado en el plano-s
punto_dc=-sigma + 1i*wd;

% Polo deseado en el plano-z
punto_dd=exp(punto_dc*Ts);

dendesz=poly([punto_dd punto_dd']);


Gdes=tf(1,poly([punto_dd punto_dd']),Ts);  
figure; pzmap(Gdes)

% Proyecto compensador por lugar de las raíces

poloscompz=[0 0]; % Un polo al origen del PD convencional

% Se obtiene el denominador del compensador PD en tiempo discreto
dencompz=poly(poloscompz); 

% Numerador y denominador de la funcion de transferencia de lazo abierto
% sin el cero del compensador
numftlaz=numd;
denftlaz=conv(dend,dencompz);

cerosftlaz=roots(numftlaz);
polosftlaz=roots(denftlaz);

% Glad=tf(numftlaz,denftlaz,Ts);
% figure; rlocus(Glad)

for k=1:numel(polosftlaz);
    dif_menos(k) = punto_dd - polosftlaz(k);
    fi_menos(k)= angle(dif_menos(k))*180/pi;
end
sumfi_menos=sum(fi_menos);

for k=1:numel(cerosftlaz);
    dif_mas(k) = punto_dd - cerosftlaz(k);
    fi_mas(k)= angle(dif_mas(k))*180/pi;
end
sumfi_mas=sum(fi_mas);
% sumfi_mas=0;

sumadeangulos=sumfi_mas-sumfi_menos;

% Condicion de angulo
deficiencia=-sumadeangulos;        % parte negativa eje real plano-z 
% deficiencia=-sumadeangulos+180;  % parte positiva eje real plano-z

x=tan(deficiencia*pi/180);
cero_compz=(imag(punto_dd)/x) - real(punto_dd);  % parte negativa eje real plano-z

% Funcion de transferencia del compensador PD convencional 
numcompz=poly(cero_compz);
Gcz=tf(numcompz,dencompz,Ts);

% Numerador y denominador de la funcion de transferencia de lazo abierto
% completa
numftlaz=conv(numd,numcompz);
denftlaz=conv(dend,dencompz);

Glad_comp=tf(numftlaz,denftlaz,Ts);

% Fijado el cero del compensador, debe calcularse la ganancia del PD por la condicion de módulo

for k=1:numel(denftlaz);
    modulo_den(k)=denftlaz(k)*punto_dd^(numel(denftlaz)-k);
end
modulo_den=sum(modulo_den);

for k=1:numel(numftlaz);
    modulo_num(k)=numftlaz(k)*punto_dd^(numel(numftlaz)-k);
end
modulo_num=sum(modulo_num);

% Condicion de magnitud
Kpd=-abs(modulo_den)/abs(modulo_num);
% Kpd=abs(modulo_den)/abs(modulo_num);

a=cero_compz;   % cambio de signo para no tener que restar

% Gcpd=Kpd*(z+a)/z;
aKpd=a*Kpd;
z=tf('z',Ts);

k1=Kpd;
k2=a*Kpd;

Gcpd=Kpd*(z - a)/z^2;

Glad_comp=series(Gcpd,Gpd);

close all
figure; margin(Glad_comp)

Glc_cPD=feedback(Glad_comp,1);
figure; step(Glc_cPD)
figure; pzmap(Glc_cPD)


% Proyecto del controlador por modelo interno
d=4;    % Numero de adelantos para corregir la fase de la planta

% Modelo interno con armonicas impares
if mi_aimp == 1
    Qmi=0.999;   % Filtro pasa bajos 
    kmi=0.25;    % Ganancia del controlador por MI
    Nbuffer=N/2; % Buffer del modelo interno
    Gc_mi=kmi*z^d/(z^(Nbuffer) + Qmi);
end

% Modelo interno con armonicas pares e impares
if mi_apim == 1
    Qmi=0.98;  % Filtro pasa bajos 
    kmi=0.1;   % Ganancia del controlador por MI
    Nbuffer=N;  % Buffer modelo interno
    Gc_mi=kmi*z^d/(z^(Nbuffer) - Qmi);
end

Gc_mi=minreal(Gc_mi);
sys_mi=tf(Gc_mi);

% figure; pzmap(Gc_mi)
% figure; rlocus(Gc_mi)

% Qz1=0.25*z+0.5+0.25*z^(-1);
% Qz2=0.125*z+0.75+0.125*z^(-1);
% Qz3=0.05*z+0.9+0.05*z^(-1);
% figure; bode(Qz1,Qz2,Qz3)

% Gc_mi=kmi*z^d/(z^(N/2) - Qz1);

Gc_total=parallel(Gc_mi,Gcpd);
Glad=series(Gc_total,Gpd);
figure; margin(Glad)

Glc_total=feedback(Glad,1);
figure; step(Glc_total)

% FT con accion feedforward
% Glcfeed = Gpd*(1 + Gc_total)/(1 + Gpd*Gc_total);
% figure; step(Glcfeed);
% legend('sistema compensado con feedforward de la referencia');

%% Analisis de estabilidad
Krlocus=0:0.1:1;
figure; rlocus(Glad,Krlocus)
[r] = rlocus(Glad,Krlocus);
hold on;
% plot(r(numel(r)/numel(Krlocus),numel(Krlocus)),'*r'); % para ver la ubicación final del ultimo polo
axis([-1.5 1.5 -1.5 1.5])
for i=1:(numel(r)/numel(Krlocus)) % este for es para ir incrementando la evaluación de la ubicación final de cada polo de la función de lazo abierto 
    coordenadas=r(i,numel(Krlocus));% nos da la ubicación final en real e imaginario de cada polo
    modulo=abs(coordenadas);
    if modulo > 1
        estable(i) = 0; % se guarda en un vector el resultado de la evaluación, para posteriormente determinar la estabilidad final 
                        % (si se encuentra un 0 el sistema no es estable)
        if imag(r(i,numel(Krlocus))) == 0
            coordenadas=r(i,numel(Krlocus))+0.0000001*1i; % para salvar cuando parte imaginaria da cero
        end
        plot(coordenadas,'*r','LineWidth',3)
    else
        estable(i) = 1;
    end
end

 
t=0:Tmi:30*Tr-Tmi;
ref=311*sin(2*pi*fr*t);

figure;
lsim(Glc_total,ref,t);
% axis([0.5 0.6 -350 350])
legend('sistema compensado sin feedforward de la referencia');

% figure;
% lsim(Glcfeed,ref,t);
% legend('sistema compensado con feedforward de la referencia');

[y,t]=lsim(Glc_total,ref,t);
error=ref' - y;
figure; plot(t,error)
% axis([0.5 0.6 -350 350])