clear
close all

%% Se diseña e implementa un PD predictivo para el control de la tensión de salida
%% de un inversor monofásico PWM.
%% Se debe diseñar el PD para cumplir con las siguientes especificaciones de desempeño:
%% Sobrepaso menor o igual al 20% y tiempo de asentamiento de 5 ms para entrada en escalón
%% Error de posición nulo para entrada en escalón y error de seguimiento reducido para entrada sinusoidal.

s=tf('s');
% Parámetros de la planta
Lf=1e-3; Cf=60e-6; R=48.4; fr=50;
% Función de transferencia de la planta en tiempo continuo
Gpc=1/(Lf*Cf)/(s^2 + s/(R*Cf) + 1/(Lf*Cf));

wn=1/sqrt(Lf*Cf); fn=wn/(2*pi); xita=1/(2*wn*R*Cf);
% Periodo de muestreo seleccionado en base a la regla obtenida para el diseño del controlador OSAP
fm=8000;
T=1/fm;      % 125 us;
z=tf('z',T);
% Función de transferencia de la planta en tiempo discreto aproximada por ZOH
Gpd=c2d(Gpc,T);

% Para el diseño ir al script de Mathcad: "Diseño_PID_Discreto_Pole_Placement.xmcd"
% Especificaciones en tiempo continuo
Mp = 2/100; ts=2e-3;
xita = 1/((-pi/log(Mp))^2 + 1)^0.5;
wnc=4/(xita*ts);
s1 = -xita*wnc + 1i*wnc*sqrt(1 - xita^2);
z1 = exp(s1*T);

% Se toma xita=0.7 y wnc=3000 rad/s
% Las ganancias calculadas por reubicación de polos resultan
K1=-0.68435; %-0.20742;
K2=-0.05836; %-0.46639;
% Y los polos no dominantes que se obtienen son:
pnd1=-0.05115; pnd2=0.23293;
% El cero del compensador PD predictivo se ubica en:
zc=K2/K1;

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

Gla=minreal(Gpd*Gcd);
figure; margin(Gla); grid
legend('Estabilidad de LC en tiempo discreto con PD predictivo')
Glc=feedback(Gla,1);

figure; pzmap(Gla);
legend('Mapa de polos y ceros de LA en tiempo discreto con PD predictivo')

figure; bode(Glc); grid
legend('Desempeño de LC en tiempo discreto con PD predictivo')

% Respuesta a una entrada en escalón
te=0:T:0.006;
figure; step(Glc,te); grid
legend('Respuesta al escalón unitario con PD predictivo')
ysf=step(Glc,te); 
rst=ones(length(te),1);
est=rst - ysf;
figure; plot(te,est)
legend('Señal de error con PD predictivo')
figure; lsim(Gcd,est,te); grid
legend('Acción de control PD predictivo')

% Respuesta a una entrada senoidal
nc=4; f1=50; T1=1/f1; tf=nc*T1;
tsim=0:T:tf;
u=sin(2*pi*f1*tsim);
figure; lsim(Glc,u,tsim); grid
legend('Respuesta a entrada sinusoidal con PD predictivo')

%% Simulación con acción feedforward
% Glafeedfor=minreal(Gpd*(1 + Gcd));
% figure; bode(Glafeedfor); grid
% legend('Estabilidad de LC en tiempo discreto con PD predictivo')

Glcfeedfor=minreal(Gpd*(1 + Gcd)/(1 + Gcd*Gpd));
figure; bode(Glcfeedfor); grid
legend('Desempeñp de LC con PD predictivo + feedforward')

% Para entrada en escalón
figure; step(Glcfeedfor,te); grid
legend('Respuesta al escalón unitario con PD predictivo + Feedforward')
ycf=step(Glcfeedfor,te);
ecf=rst - ycf;
figure; stairs(te,ecf); grid
legend('Señal de error con PD predictivo + feedforward')

figure; lsim(Gcd,ecf,te); grid
legend('Acción de control PD predictivo')
ucf=rst+lsim(Gcd,ecf,te);
figure; stairs(te,ucf)
legend('Acción de control PD predictivo + feedforward')

% Para entrada sinusoidal
figure; lsim(Glcfeedfor,u,tsim); grid
legend('Respuesta a entrada sinusoidal con PD predictivo + Feedforward')
ysin_f=lsim(Glcfeedfor,u,tsim);
esin_f=u - ysin_f';
figure; stairs(tsim,esin_f); grid
legend('Señal de error con PD predictivo + feedforward')

usin_f=u'+lsim(Gcd,esin_f,tsim);
usin=lsim(Gcd,esin_f,tsim);
figure; stairs(tsim,usin_f); hold on
stairs(tsim,usin); grid
legend('Acción de control PD predictivo + feedforward')

close all
%% Ajuste con SISOTOOL
  
Gcdaj = -0.73507*(z + 0.08528)/z^2;
zca = 0.08528;
K1a = -0.73507; K2a = zca*K1a;

% Glafeedforaj=minreal(Gpd*(1 + Gcdaj));
% figure; bode(Glafeedforaj); grid
% legend('Estabilidad de LC en tiempo discreto con PD predictivo')

Glcfeedforaj=minreal(Gpd*(1 + Gcdaj)/(1 + Gcdaj*Gpd));
figure; bode(Glcfeedfor); grid
legend('Desempeñp de LC con PD predictivo + feedforward')

% Para entrada en escalón
figure; step(Glcfeedforaj,te); grid
legend('Respuesta al escalón unitario con PD predictivo + Feedforward')

ycf=step(Glcfeedforaj,te);
ecf=rst - ycf;
figure; stairs(te,ecf); grid
legend('Señal de error con PD predictivo + feedforward')

figure; lsim(Gcdaj,ecf,te); grid
legend('Acción de control PD predictivo')
ucf=rst+lsim(Gcdaj,ecf,te);
figure; stairs(te,ucf)
legend('Acción de control PD predictivo + feedforward')

% Para entrada sinusoidal
figure; lsim(Glcfeedforaj,u,tsim); grid
legend('Respuesta a entrada sinusoidal con PD predictivo + Feedforward')

close all
%% Matrices de estado de la planta para simulación digital
% Vector de estados: x=[iL;vo] 
A=[ 0     -1/Lf
   1/Cf  -1/R/Cf];
     
B=[1/Lf;0];

C=[0 1]; D=0;

[G,H]=c2d(A,B,T);

% Inicialización de variables
nc=2; fs=1/T; ms=fs/fr;
m=nc*ms;
y=zeros(1,m);
r=zeros(1,m);
t=zeros(1,m);
e=zeros(1,m);
upd=zeros(1,m);
x=zeros(2,m);
vo=zeros(1,m);

% Cambiando el flag de 0 a 1 se selecciona la referencia a aplicar al control
flag_step = 0; flag_sine = 1;

for k=1:m
    t(k)=k*T;
    
    r(k)=0.5;
    if flag_step == 1
        if k>=0.5*m
            r(k)=1;
        end
    end
    
    if flag_sine == 1
        r(k)=1*sin(2*pi*fr*T*k);
    end
    
    y(k)=C*(x(:,k)/311);
    e(k)=r(k)-y(k);
    
    if k>2
        upd(k)=K1a*e(k-1) + K2a*e(k-2) + 1*r(k);
    end
    vo(k)=x(2,k);
    
    x(:,k+1)=G*x(:,k) + H*upd(k)*311;
end
if flag_sine == 1
    figure; plot(t,vo,'k',t,311*r(1:k),'r'); grid; legend('salida','referencia'); axis([0 t(length(t)) -320 320])
    figure; plot(t,e); grid; legend('error'); axis([0 t(length(t)) -1.1 1.1])
end

if flag_step == 1
    figure; plot(t,vo,'k'); hold on; plot(t,311*r(1:k),'r'); grid; 
    legend('salida','referencia'); axis([0 t(length(t)) 0 320])
    figure; stairs(t,y,'k'); hold on; stairs(t,e,'r')
    grid; legend('salida','error'); axis([0 t(length(t)) -0.15 1.1])
end

figure; plot(t,upd(1:k),'r'); grid; legend('acción de control')
figure; plot(t,x(1,1:k),'r'); legend('corriente inductor x1')