%%
clear
close all

s=tf('s');
% Función de transferencia del proceso
Gpc=200/((s + 3)*(s + 10)^2);

% Se define vector de tiempo continuo para simulación
tc=0:0.001:3.0-0.001;
% Se obtiene la función de transferencia en lazo cerrado sin compensador
% y su correspondiente respuesta al escalón unitario
Gpclc=feedback(Gpc,1);
figure; step(Gpclc,tc);
title('Respuesta al Escalón Unitario a LC sin Compensación')

% Se extrae de la información de la respuesta al escalón, el tiempo de
% asentamiento para límite o tolerancia deseada
ST=0.01;  % tolerancia del 5, 2 o 1 %
spec=stepinfo(Gpclc,'SettlingTimeThreshold',ST);
ts95=spec.SettlingTime;

% Relación empírica para obtener el periodo de muestreo
T=0.2*ts95;
% T=0.02;

% Obtención de la función de transferencia muestreada de la planta
z=tf('z',T);
Gpd=c2d(Gpc,T);
% Se define vector de tiempo a intervalos discretos para simulación
td=0:T:3.0;
% Se obtiene la función de transferencia en lazo cerrado en tiempo discreto
% sin compensador y su correspondiente respuesta al escalón unitario
% la cual se compara con el mismo sistema en tiempo continuo
Gpdlc=feedback(Gpd,1);
figure; step(Gpclc,Gpdlc,td);
title('Respuesta al Escalón Unitario a LC sin Compensación')
legend('Sistema en Tiempo Continuo','Sistema en Tiempo Discreto')

% if T == 0.2
%     td=0:T:1.6;
% else
%     td=0:T:0.26;
% end

% Obtención de los coeficientes del numerador y denominador de la planta muestreada
% F=Gpd.num; D=Gpd.den;
% n=F{:,:}; d=D{:,:};
[n,d]=tfdata(Gpd,'v');
b1=n(2); b2=n(3); b3=n(4);
a1=d(2); a2=d(3); a3=d(4);

% Diseño de los coeficientes del controlador Deadbeat
q0=1/(b1 + b2 + b3); q1=q0*a1; q2=q0*a2; q3=q0*a3;
p1=q0*b1; p2=q0*b2; p3=q0*b3;

% Polinomios P(z) y Q(z)
Qz=minreal(q0 + q1*z^-1 + q2*z^-2 + q3*z^-3);
Pz=minreal(p1*z^-1 + p2*z^-2 + p3*z^-3);

% Obtención de la función de transferencia en tiempo discreto del controlador Deadbeat
Gcd=minreal(Qz/(1 - Pz));

% Función de transferencia en tiempo discreto de lazo cerrado
% Glad=minreal(series(Gcd,Gpd));
% Glcd=minreal(feedback(Glad,1));
Glcd=Pz;

% Se almacena el vector de salida de la planta para una referencia en escalón unitario
[yk]=step(Glcd,td);
% Se define un vector de referencia unitaria en escalón
rk=ones(length(td),1);
% Obtención del vector de error
ek=rk - yk;
% Obtención de la acción de control en tiempo discreto
figure; lsim(Gcd,ek,td);
title('Acción de control Deadbeat')

% Comparación de la respuesta en lazo cerrado sin compensación en tiempo continuo
% con la respuesta en lazo cerrado compensado en tiempo discreto
figure; step(Glcd,td,'r'); hold on; step(Gpclc,tc); grid;
legend('Sistema en Tiempo Discreto a LC con Deadbeat','Sistema en Tiempo Continuo a LC Sin Compensar')

figure; bode(Glcd)
figure; margin(Gcd*Gpd)
close all
%% Simulación digital
% Inicialización de las variables utilizadas en el programa
m=length(td);
y=zeros(1,m);
r=zeros(1,m);
e=zeros(1,m);
t=zeros(1,m);
u=zeros(1,m);
yk=yk'; % Obtiene el vector traspuesto de la señal de salida obtenida con la función step
i=0;    % Variable auxiliar utilizada para recorrer el vector yk

for k=4:m
    i=i+1;
    t(k)=(k-4)*T;
    r(k)=1;
    y(k)=yk(i);
    e(k)=r(k) - y(k);
    u(k)=p1*u(k-1) + p2*u(k-2) + p3*u(k-3) + q0*e(k) + q1*e(k-1) + q2*e(k-2) + q3*e(k-3);
end

figure; stairs(t,y,'b'); hold on; plot(t,y,'ko'); grid; axis([0 max(t) 0 1.1]);
legend('Salida de la Planta')
figure; stairs(t,e,'r'); hold on; plot(t,e,'ko'); grid; axis([0 max(t) 0 1.1]);
legend('Señal de Error')
figure; stairs(t,u,'k'); hold on; plot(t,u,'ro'); grid; axis([0 max(t) min(u)-0.5 max(u)+0.5]);
legend('Acción de control Deadbeat')