%% Se diseña y simula el control One Sampled Ahead Preview (OSAP) para una planta o proceso de segundo orden
%% en este caso, el de un filtro pasa bajos LC, el cual puede ser aplicado a la salida de una fuente CC-CC o CC-CA

clear
close all

s=tf('s');
% Parametros del filtro LC
L=500e-6; C=60e-6; R=5; fr=50;
wn=1/sqrt(L*C); fn=wn/(2*pi); xita=1/(2*wn*R*C);
% Función de transferencia de la planta
Gpc=(1/(L*C))/(s^2 + s*(1/(R*C)) + 1/(L*C));
% Función de transferencia de la planta en lazo cerrado sin compensación
% y respuesta al escalón
Gpclc=feedback(Gpc,1);
figure; step(Gpclc);

% Obtención del periodo de muestreo diseñado a partir de la frecuencia natural no
% amortiguada
Td=0.2/fn; 
% Periodo adoptado por aproximación para tener un número entero del registro del timer
T=200e-6;
fm=1/T;

z=tf('z',T);
% Obtención de la función de transferencia muestreada de la planta
Gpd=c2d(Gpc,T);

% Gpd =
%  
%      0.4854 z + 0.385
%   -----------------------
%   z^2 - 0.6429 z + 0.5134
%  
% Sample time: 0.0002 seconds
% Discrete-time transfer function.

% Función de transferencia de lazo cerrado sin compensación en tiempo discreto y
% Respuesta al escalón
Gpdlc=feedback(Gpd,1);
figure; step(Gpdlc);

[numd,dend]=tfdata(Gpd,'v');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Se diseña un Deadbeat para verificar la respuesta
b1=numd(2); b2=numd(3);
a1=dend(2); a2=dend(3);

q0=1/(b1 + b2); q1=q0*a1; q2=q0*a2;
p1=q0*b1; p2=q0*b2;

% Polinomios en z
Qz=minreal(q0 + q1*z^-1 + q2*z^-2);
Pz=minreal(p1*z^-1 + p2*z^-2);

Gcd=minreal(Qz/(1 - Pz)); figure;
Glcd=Pz; step(Glcd,'r'); hold on; step(Glcd,'bo'); 
grid; title('Respuesta al escalón con Deadbeat')
tstep=0:T:2e-3;
[yk]=step(Glcd,tstep);
rstep=ones(length(tstep),1);
ek=rstep - yk;
figure; lsim(Gcd,ek,tstep);
title('Acción de control Deadbeat')

tseno=0:T:(2/50);
ref=1*sin(2*pi*50*tseno);
figure; lsim(Glcd,ref,tseno); grid
title('Respuesta a entrada sinusoidal con Deadbeat')
[yk]=lsim(Glcd,ref,tseno);
ek=ref - yk';
figure; lsim(Gcd,ek,tseno);
title('Acción de control Deadbeat')
figure; stairs(tseno,ek);
title('Señal de error con controlador Deadbeat')
%% Se observa que para una referencia sinusoidal el controlador Deadbeat funciona bien
%% no obstante presenta un error de régimen permanente debido a que el controlador fue
%% diseñado para una referencia en escalón.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Parametros del controlador OSAP
p1=a1; p2=a2; q1=b1; q2=b2;
%% Parametros del controlador OSAP modificado
p1m=(p2-a1*p1)/q1;
p2m=-p1*a2/q1;
q1m=1/q1;
q2m=(p1*b1-q2)/q1;
q3m=p1*b2/q1;

G1=(1/(1 - q2m*z^-1 - q3m*z^-2));
Gla=Gpd*G1;
G2=p1m*z^-1 + p2m*z^-2;
Glci=feedback(Gla,G2,1);
Glc=q1m*z*Glci;

% Evaluación de la respuesta al escalón
desp = 2;                         % Nº de periodos que se demora en aplicar la referencia
A = zeros(1,desp);                % Crea vector de ceros.
B = ones(1,(length(tstep)-desp)); % Crea vector de unos.
rstep = horzcat(A,B);             % Cocatena vectores A y B para obtener escalón desplazado.
yosap=lsim(Glc,rstep,tstep);
figure; stairs(tstep,yosap); grid
axis([0 8*T 0 1.1])

% Evaluación de la respuesta con referencia sinusoidal
figure; lsim(Glc,ref,tseno); grid
yseno=lsim(Glc,ref,tseno)';
error_seno=ref-yseno;
figure; stairs(tseno,error_seno); hold on; stairs(tseno,yseno)
legend('Señal de error','Salida')
title('Señal de error y Salida con controlador OSAP Modificado')

% Para la accion de control
yfeedback=lsim(G2,yseno,tseno);
% figure; stairs(tseno,yfeedback)
r_avanz=q1m*(ref+T);
e_G1=(r_avanz)+yfeedback';
% figure; stairs(tseno,e_G1)

u=lsim(G1,e_G1,tseno);
figure; stairs(tseno,u); grid
axis([0 max(tseno) -1.0 1.0])
title('Acción de control OSAP Modificado')

close all
%% Hasta aqui una forma de simularlo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nc=4;
tf=nc/fr;
tsin=0:T:tf;
ref=sin(2*pi*fr*tsin);
Gosap=Glc;
figure; lsim(Gosap,ref,tsin);
ref_step=ones(length(tsin),1);
figure; lsim(Gosap,ref_step,tsin);
axis([0 max(tsin) 0 1.1])
% [vo,tsin]=lsim(Gosap,ref,tsin);
% vo=vo';
% figure; plot(tsin,vo);

rr=1;  % seno

% Representación en el espacio de estado de la planta
% Los estados son la tension en el capacitor y su derivada
A=[ 0        1
  -wn^2  -2*xita*wn];

B=[0;wn^2]; C=[1  0]; D=0;
[G,H]=c2d(A,B,T);

% Inicialización de variables
m=length(tsin);
y=zeros(1,m);
r=zeros(1,m);
e=zeros(1,m);
t=zeros(1,m);
uosap_m=zeros(1,m);
x=zeros(2,m);
i=0;
limite=1.0;

for k=3:m
    t(k)=(k-3)*T;
    i=i+1;
    
    r(k)=ref_step(i);
    r(k+1) = ref_step(i+1);
    if rr == 1
        r(k)=ref(i);
        r(k+1) = ref(i+1);
    end
    e(k)=r(k)-y(k);
    
    uosap_m(k)=q1m*r(k+1) + p1m*y(k-1) + p2m*y(k-2) + q2m*uosap_m(k-1) + q3m*uosap_m(k-2);
    x(:,k+1)=G*x(:,k) + H*uosap_m(k);
    y(k+1)=C*x(:,k+1);
end

if rr == 1
    figure; stairs(t(2:k),y(2:k),'k'); hold on; stairs(t(2:k),r(2:k),'r'); grid; axis([0 max(t) -1.1 1.1])
    legend('Salida','Referencia')
    figure; stairs(t,uosap_m,'r'); grid; legend('acción de control'); axis([0 max(t) -1.1 1.1])
    figure; stairs(t,e,'k'); grid; legend('error'); axis([0 max(t) -1 1])
end

if rr == 0
    figure; stairs(t(2:k),y(2:k),'k'); hold on; stairs(t(2:k),r(2:k),'r'); grid; %axis([0 max(t) -1.1 1.1])
    legend('Salida','Referencia')
    figure; stairs(t,uosap_m,'r'); grid; legend('acción de control'); %axis([0 max(t) -1.1 1.1])
    figure; stairs(t,e,'k'); grid; legend('error'); %axis([0 max(t) -1 1])
end