clear
close all

s=tf('s');
L=1e-3; C=25e-6; R=12; fr=50;
% L=250e-6; C=240e-6; R=3; fr=50;
Gpc=1/(L*C)/(s^2 + s/(R*C) + 1/(L*C));

wn=1/sqrt(L*C); fc=wn/(2*pi); xita=1/(2*wn*R*C);

fm=10000;

T=1/fm;
tstep=0:T:2e-3;
% 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);

z=tf('z',T);
Gpd=c2d(Gpc,T);
figure; axis square; pzmap(Gpd);

% Gpd =
%  
%     0.1171 z + 0.1047
%   ----------------------
%   z^2 - 1.495 z + 0.7165
%  
% Sample time: 0.0001 seconds
% Discrete-time transfer function.

F=Gpd.num; D=Gpd.den;
n=F{:,:}; d=D{:,:};
b1=n(2); b2=n(3);
a1=d(2); a2=d(3);

% b1=0.004902; b2=0.004817;
% a1=-1.939; a2=0.9487;
% T=100e-6;
% z=tf('z',T);
% if T==100e-6
%     b1=0.1737; b2=0.1553;
%     a1=-1.388; a2=0.7165;
% end

% Parametros del controlador OSAP
p1=a1; p2=a2; q1=b1; q2=b2;

% Glc
Glc=((b1*z + b2)*z^2)/((z^2 + a1*z + a2)*(q1*z + q2)-(p1*z + p2)*(b1*z + b2));
figure; axis square; pzmap(Glc);

% Evaluación de la respuesta al escalón
desp = 5;                         % 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
hold on
plot(tstep,yosap,'ok');
axis([0 max(tstep) 0 1.1])

% [y,td]=step(Gosap); figure; plot(td,y,'ok'); axis([0 td(length(td)) 0 1.1])

% Closed-loop transfer function: Gosap
% 0.1737 z^3 + 0.1553 z^2
% -----------------------
% 0.1737 z^3 + 0.1553 z^2
%  
% Sampling time: 0.0001


% Variacion parametrica
L=1000e-6; C=80e-6; R=5; wn=1/sqrt(L*C); 
fn=wn/(2*pi); xita=1/(2*wn*R*C);
A=[ 0        1
  -wn^2  -2*xita*wn];

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

sysd_varp=ss(G2,H2,C,D,T);
figure; axis square; pzmap(Gpd,sysd_varp,Glc)
legend('Planta Discreta Nominal','Planta Discreta VarPar','Glc')

% Inicialización de variables
nc=12; fs=1/T; ms=round(fs/fr);
m=nc*ms;

y=zeros(1,m);
r=zeros(1,m);
rkm1=zeros(1,m);
e=zeros(1,m);
t=zeros(1,m);
uosap=zeros(1,m);
x=zeros(2,m);

td=0:T:(nc/fr)-T;
% Referencias
r_step=ones(1,length(td));
r_seno=sin(2*pi*fr*td);

flag_planta = 1;  % Si es igual a 1, simula variacion parametrica
i=0;

for k=2:m
    i=i+1;
    t(k)=(k)*T;
    
%     r(k)=ref(i);
%     r(k+1) = ref(i+1);
        
    r(k) = 0.5*r_step(i);
    rkm1(k) = 0.5*r_step(i+1);
    if k >=0.5*m
        r(k) = r_step(i);
        rkm1(k) = r_step(i+1);
    end
    
%     r(k) = 0.5*r_seno(i);
%     rkm1(k) = 0.5*r_seno(i+1);
%     if k >=0.5*m
%         r(k) = r_seno(i);
%         rkm1(k) = r_seno(i+1);
%     end
    
    y(k)=C*x(:,k);
    e(k)=r(k)-y(k);

    uosap(k)=(1/q1)*rkm1(k) + (p1/q1)*y(k) + (p2/q1)*y(k-1) - (q2/q1)*uosap(k-1);
    
    if flag_planta == 1
        Gs=G2; Hs=H2;
    else
        Gs=G; Hs=H;
    end
    x(:,k+1)=Gs*x(:,k) + Hs*uosap(k);
end
% figure; plot(t(2:k),y(2:k),'k',t(2:k),r(2:k),'r'); grid; %axis([0 0.002 0 1.1])
% figure; plot(t,uosap,'r'); grid; legend('acción de control'); %axis([0 0.12 -1.1 1.1])
% figure; plot(t,x(1,1:k),'r'); legend('tensión salida x1=y')
% figure; plot(t,x(2,1:k),'k'); legend('derivada tensión salida x2')
% figure; plot(t,e,'k'); grid; legend('error'); axis([0 0.12 -1 1])

figure; stairs(t(1:k),y(1:k),'b'); hold on; stairs(t(1:k),r(1:k),'k'); grid;
legend('salida','referencia')
figure; stairs(t,uosap,'r'); grid; legend('acción de control');
figure; stairs(t,e,'k'); grid; legend('error'); axis([0 max(t) -1 1])