clear
close all
s=tf('s');
T=0.02;
z=tf('z',T);

tf=10;
t=0:T:tf-T;

Gp=1/(s*(s+2));
Gplc=feedback(Gp,1);

num=1; den=[1 2 0];
[A,B,C,D]=tf2ss(num,den);
[G,H]=c2d(A,B,T);
Kc=9; zc=2; pc=3;

Gc=Kc*(s+zc)/(s+pc);
Gla=Gc*Gp;
Glc=feedback(Gla,1);
% figure; step(Glc,t);
% legend('Planta Continua CC')

Gpd=c2d(Gp,T);
Gcd=(9*z-6*exp(-3*T)-3)/(z-exp(-3*T));
Glad=Gcd*Gpd;
Glcd=feedback(Glad,1);
k1=9; k2=6*exp(-3*T)+3; k3=exp(-3*T);

np=tf/T;
% Inicialización de variables
y=zeros(1,np);
r=zeros(1,np);
e=zeros(1,np);
t=zeros(1,np);
u=zeros(1,np);
x=zeros(2,np);

for k=2:np
    t(k)=(k-2)*T;
    r(k)=1;
    y(k)=C*x(:,k);
    e(k)=r(k)-y(k);
    u(k)=k1*e(k)-k2*e(k-1)+k3*u(k-1);
    x(:,k+1)=G*x(:,k) + H*u(k);
end

figure; step(Glc); hold on
stairs(t(1:k),y(1:k),'k');
plot(t(1:k),y(1:k),'or'); 
grid;
legend('Salida Sistema Continuo','Salida Sistema Discreto con ZOH','Salida Sistema Digital');
axis([0 max(t) 0 1.4])

figure;
stairs(t(1:k),u(1:k)); hold on
plot(t(1:k),u(1:k),'*r')
legend('acción de control');

figure;
stairs(t(1:k),x(2,1:k))
% figure;
% stairs(t(1:k),e(1:k)); hold on
% stairs(t(1:k),y(1:k))
% legend('error','salida');