clc; clear
close all

s=tf('s'); 
tfinal=10;      % Tiempo final de simulación

% Planta
Gp=1080/(s*(s+6)*(s+18));

syslcc_nc=feedback(Gp,1);
% figure; step(syslcc_nc);

% Access with a Structure field access (.)
spec=stepinfo(syslcc_nc);
%         RiseTime: 0.1833
%     SettlingTime: 2.8384
%      SettlingMin: 0.7211
%      SettlingMax: 1.5140
%        Overshoot: 51.3964
%       Undershoot: 0
%             Peak: 1.5140
%         PeakTime: 0.4854

tr_sc=spec.RiseTime;        % tiempo de subida sistema lazo cerrado sin compensación
ts_sc=spec.SettlingTime;    % tiempo de asentamiento sistema lazo cerrado sin compensación
tp_sc=spec.PeakTime;        % tiempo de pico sistema lazo cerrado sin compensación
Mp_sc=spec.Overshoot;       % sobrepaso sistema lazo cerrado sin compensación

% Compensador de adelanto diseñado en tiempo continuo
% Especificaciones de desempeño exigidas: Mp de aproximadamente el 3% y 
% tiempo de asentamiento menor o igual a 1 segundo
Kc=1;
Gc=Kc*(s + 4)/(s + 10);

paso_sim=5e-6;
tc=0:paso_sim:tfinal-paso_sim;

Gla=Gc*Gp;
gan_la_sc=dcgain(Gla);
Glc=feedback(Gla,1);

spec=stepinfo(Glc);
tr_cc=spec.RiseTime;        % tiempo de subida sistema lazo cerrado con compensación

figure; step(syslcc_nc,Glc,tc)
title('Desempeño del Sistema en Tiempo Continuo')
legend('Sin Compensar','Compensado');

figure; margin(Gla)
legend('Estabilidad Sistema Compensado');

% figure; bode(syslcc_nc,Glc); grid
% legend('Sin Compensar','Compensado');

wBWsc = bandwidth(Glc);
fBWsc = wBWsc/2/pi;
fm = 30*fBWsc;
T = 1/fm;
T = 0.025;
%% Determinación del periodo de muestreo en base a las especificaciones 
% del sistema de control en lazo cerrado sin compensación
% Nm = tr/T
% Nm=12;              % Número de muestras
% T=tr_cc/Nm;         % Periodo de muestreo en base al tiempo de subida

z=tf('z',T);
Gpd=c2d(Gp,T,'zoh');
tm=0:T:tfinal-T;

% figure; pzmap(Gpd)

% Gpd =
%  
%   0.002871 z^2 + 0.009832 z + 0.002089
%   ------------------------------------
%    z^3 - 2.473 z^2 + 2.003 z - 0.5293
%  
% Sample time: 0.026509 seconds
% Discrete-time transfer function.

%% Discretizacion del controlador en tiempo continuo usando aproximación de Tustin
% para verificar el efecto de la compensación polo-cero
Gcd=c2d(Gc,T,'tustin');
% Compensación ZOH
epsilon=0.1;
Cz=2*(z-epsilon)/(z + 1 - 2*epsilon);
% Compensador equivalente que incluye compensación polo-cero C(z)
Dz=Gcd*Cz;
% FTLA SIN compensación ZOH
Glad_scpz=minreal(Gcd*Gpd);
% FTLA CON compensación ZOH
Glad_cpz=minreal(Dz*Gpd);

%% Análisis
%% Verificar estabilidad y desempeño del sistema discreto en LC SIN compensación del ZOH
% FTLC SIN compensación ZOH
Glcd_scpz=feedback(Glad_scpz,1);

figure; step(Glc,Glcd_scpz,tm);
legend('Sistema compensado tiempo continuo','Sistema compensado tiempo discreto SC Polo-Cero');

figure; bode(Gla,Glad_scpz); grid
legend('Estabilidad SC tiempo continuo','Estabilidad SC tiempo discreto sin Cz');

%% Verificar estabilidad y desempeño del sistema discreto en LC CON compensación del ZOH
Glcd_cpz=feedback(Glad_cpz,1);

figure; step(Glc,Glcd_cpz);
legend('Sistema compensado tiempo continuo','Sistema compensado tiempo discreto CC Polo-Cero');

figure; bode(Gla,Glad_cpz); grid
legend('Estabilidad SC tiempo continuo','Estabilidad SC tiempo discreto con Cz');

% Verificar desempeño en frecuencia del sistema en tiempo continuo y del
% sistema en tiempo discreto compensado con y sin compensación del ZOH

figure; bode(Glc,Glcd_cpz,Glcd_scpz); grid
legend('Sistema continuo','Sistema con compensacion del ZOH','Sistema sin compensacion del ZOH');
% Ancho de banda conseguido en tiempo continuo es de aprox 8 rad/seg. Con
% la compensación polo-cero se consigue casi 8,3 rad/seg.

%% Acción de control con compensación ZOH
Gu=feedback(Dz,Gpd);
figure; step(Gu,tm);
% close all

%% Simulación Digital
num=1080; den=[1 24 108 0];
[A,B,C,D]=tf2ss(num,den);
[G,H]=c2d(A,B,T);
% Coeficientes del controlador
% k1=0.7778; k2=0.2222; k3=1.867; k4=1.689;  % Compensación ZOH epsilon=0
k1=0.6222; k2=0.02222; k3=1.867; k4=1.876; k5=0.1689;    % Compensación ZOH epsilon=1

np=tfinal/T;
R=1;
% Inicialización de variables
y=zeros(1,np);
r=zeros(1,np);
e=zeros(1,np);
u=zeros(1,np);
x=zeros(3,np);
td=zeros(1,np);

for k=3:np
    td(k)=(k-3)*T;
    r(k)=R;  % Referencia = 10 rad
    y(k)=C*x(:,k);
    e(k)=r(k)-y(k);
%     u(k)=k1*u(k-2) - k2*u(k-1) + k3*e(k) - k4*e(k-1);
    u(k)=k1*u(k-2) - k2*u(k-1) + k3*e(k) - k4*e(k-1) + k5*e(k-2);
    x(:,k+1)=G*x(:,k) + H*u(k);
end
figure; step(R*Glc,tc); hold on
stairs(td(1:k),y(1:k),'k');
grid;
title('Simulación Digital - Posición Angular')
legend('Salida Sistema Continuo','Salida Sistema Discreto CC ZOH');
% axis([0 max(td) 0 12])

figure;
stairs(td(1:k),u(1:k));
legend('Acción de control con Cz');

figure;
stairs(td(1:k),1080*x(2,1:k))
legend('Velocidad Angular');