clear; clc
close all

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

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

% Sistema en LC continuo no compensado
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 de fase diseñado en tiempo continuo
% Especificaciones de desempeño exigidas: Mp menor al 1% y 
% tiempo de asentamiento menor a 1 segundo
Kc=0.58857;
Gc=Kc*(s + 4.931)/(s + 10);

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

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

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

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

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

figure; bode(syslcc_nc,Glc); grid
title('Respuesta en frecuencia de LC')
legend('Sin Compensar','Compensado');

%% Obtención del periodo de muestreo a partir del ancho de banda del sistema
%% compensado en tiempo continuo
wBWsc = bandwidth(Glc);
fBWsc = wBWsc/2/pi;
fm = 30*fBWsc;
Tm = 1/fm;
%% Adoptamos
T = 0.05;
z=tf('z',T);
%% Se obtiene la FT muestreada de la planta para poder verificar el
%% desempeño con el controlador aproximado y la compensación del ZOH
Gpd=c2d(Gp,T,'zoh');
tfinal=2;
tm=0:T:tfinal-T;
Gcdzoh=c2d(Gc,T,'zoh');

%% Para verificar el periodo de muestreo elegido
% FTLA tiempo discreto
Glad=minreal(Gcdzoh*Gpd);
% FTLC tiempo discreto
Glcd=minreal(feedback(Gcdzoh*Gpd,1));
figure; step(Glc,Glcd,tm)
legend('SC en TC','SC en TD Gcd ZOH')

%% 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'); % Cambiar 'tustin' por 'zoh'. Probar 'foh' y 'matched'
%% Compensación ZOH
epsilon=0.0;
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 
%% CON y SIN compensación del ZOH
% FTLC SIN compensación ZOH
Glcd_scpz=feedback(Glad_scpz,1);
% FTLC CON compensación ZOH
Glcd_ccpz=feedback(Glad_cpz,1);

%% Verificar desempeño
figure; step(Glc,Glcd_scpz,tm);
title('Compensador Aproximado por Tustin')
legend('SC en TC','SC en TD Sin Compensación ZOH');

figure; step(Glc,Glcd_ccpz,tm);
legend('SC en TC','SC en TD Con Compensación ZOH');

%% Verificar estabilidad 
figure; bode(Gla,Glad_scpz,Glad_cpz); grid
legend('Estabilidad SC TC','Estabilidad SC TD Sin Comp. ZOH','Estabilidad SC TD Con Comp. ZOH');

%% 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_scpz,Glcd_ccpz); grid
legend('SC en TC','Sistema sin compensacion del ZOH','Sistema con compensacion del ZOH');
wBWccZOH = bandwidth(Glcd_ccpz);
figure; bode(Gcd,Cz,Dz)
legend('Gcd Tustin','Cz','Dz')
%% Ancho de banda conseguido en tiempo continuo es de aprox 4,73 rad/seg. 
%% Con la compensación polo-cero se consiguen 4,77 rad/seg.

%% Acción de control con compensación ZOH
Gu=feedback(Dz,Gpd);
figure; step(Gu,tm);
close all
%% Simulación Digital
tc=0:paso_sim:tfinal-paso_sim;
num=1080; den=[1 24 108 0];
[A,B,Cr,D]=tf2ss(num,den);
[G,H]=c2d(A,B,T);
C=[0 0 1];
%% Coeficientes del controlador
% u(k)=b1*e(k) + b2*e(k-1) + a1*u(k-2) + a2*u(k-1) ;
[n,d]=tfdata(Dz,'v');
b1=n(1); b2=n(2); b3=n(2);
a2=d(2); a3=d(3);
% k1=0.6; k2=0.4; k3=1.0578; k4=0.8256;  % Compensación ZOH epsilon=0
k1=0.48; k2=0.2; k3=1.0578; k4=0.9314; k5=0.08256;    % Compensación ZOH epsilon=0.1

np=round(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;
    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*1080*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 1.1])

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

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

%% Discretizacion del controlador en tiempo continuo usando aproximación de ZOH
% para verificar el efecto de la compensación polo-cero
% Gcd=c2d(Gc,T,'zoh');
% epsilon=0.0;
% Cz=2*(z-epsilon)/(z + 1 - 2*epsilon);
% Dz=Gcd*Cz;
% 
% Glad_cpz=Dz*Gpd;
% gan_la_sdcpz=dcgain(Glad_cpz);
% 
% % figure; pzmap(Glad);
% % Verificar estabilidad
% figure; margin(Glad_cpz)
% legend('Estabilidad del sistema en lazo cerrado con compensacion del ZOH');
% 
% Glcd_ccpz=feedback(Glad_cpz,1);
% figure; step(Glc,Glcd_ccpz);
% title('Desempeño del Sistema en Tiempo Continuo y Discreto Compensados')
% legend('Sistema compensado tiempo continuo','Sistema compensado tiempo discreto CC Polo-Cero');
% 
% % Verificar estabilidad y desempeño del sistema discreto en lazo cerrado
% % sin compensación del ZOH
% Glad_scpz=Gcd*Gpd;
% gan_la_sdspz=dcgain(Glad_scpz);
% 
% Glcd_scpz=feedback(Glad_scpz,1);
% figure; step(Glc,Glcd_scpz);
% title('Desempeño del Sistema en Tiempo Continuo y Discreto Compensados')
% legend('Sistema compensado tiempo continuo','Sistema compensado tiempo discreto SC Polo-Cero');
% 
% % Verificar estabilidad
% figure; margin(Glad_scpz)
% legend('Estabilidad del sistema en lazo cerrado sin compensacion del ZOH');
% 
% figure; bode(Glad_cpz,Glad_scpz)
% title('Comparacion de la estabilidad del sistema con y sin compensacion del ZOH');
% legend('Sistema con compensacion del ZOH','Sistema sin compensacion del ZOH');
% 
% % Verificar desempeño en frecuencia del sistema en tiempo discreto compensado
% % con y sin compensación del ZOH
% figure; bode(Glc,Glcd_ccpz,Glcd_scpz);
% title('Desempeño en frecuencia del sistema continuo y discreto con y sin compensación del ZOH')
% legend('Sistema Continuo','Sistema con compensacion del ZOH','Sistema sin compensacion del ZOH');
% 
% close all
% %% ---
% fm = 100*fBWsc;
% T = 1/fm;
% 
% Gcd2=c2d(Gc,T,'zoh');
% epsilon=0.1;
% z=tf('z',T);
% Gpd=c2d(Gp,T,'zoh');
% 
% Cz=2*(z-epsilon)/(z + 1 - 2*epsilon);
% Dz2=Gcd2*Cz;
% 
% Glad_cpz2=Dz2*Gpd;
% gan_la_sdcpz=dcgain(Glad_cpz2);
% 
% % figure; pzmap(Glad);
% % Verificar estabilidad
% figure; margin(Glad_cpz2)
% legend('Estabilidad del sistema en lazo cerrado con compensacion del ZOH');
% 
% Glcd_cpz2=feedback(Glad_cpz2,1);
% figure; step(Glc,Glcd_cpz2);
% title('Desempeño del Sistema en Tiempo Continuo y Discreto Compensados')
% legend('Sistema compensado tiempo continuo','Sistema compensado tiempo discreto CC Polo-Cero');

% Verificar estabilidad y desempeño del sistema discreto en lazo cerrado
% sin compensación del ZOH
% Glad_scpz=Gcd*Gpd;
% gan_la_sdspz=dcgain(Glad_scpz);
% 
% Glcd_scpz=feedback(Glad_scpz,1);
% figure; step(Glc,Glcd_scpz);
% title('Desempeño del Sistema en Tiempo Continuo y Discreto Compensados')
% legend('Sistema compensado tiempo continuo','Sistema compensado tiempo discreto SC Polo-Cero');
%% ------------------------------------------------------------------------------
% close all
% % Especificaciones de proyecto
% % El tiempo de asentamiento del sistema compensado debe ser menor o igual a:
% ts=1.8;                 % ts=4/sigma
% 
% % El sobrepaso maximo deseado debe ser menor o igual a 5%
% Mp=15/100;              % Mp=exp(-pi.sigma/wd)
% 
% sigma=4/ts;             % Parte real de los polos dominantes de lazo cerrado
% wd=-pi*sigma/log(Mp);   % Parte imaginaria de los polos dominantes de lazo cerrado
%          
% s1=-sigma+1i*wd;         % Polos dominantes de lazo cerrado
% s2=-sigma-1i*wd;         % complejos conjugados en el dominio continuo
% 
% z1=exp(s1*T);           % Polos dominantes de lazo cerrado
% z2=exp(s2*T);           % complejos conjugados en el dominio discreto
% 
% % Polos y ceros de la planta discreta;
% % [polos,ceros]=pzmap(Gpd);
% PP=pole(Gpd);
% ZZ=zero(Gpd);
% pp1=PP(1,1);            % polo planta
% pp2=PP(2,1);            % polo planta
% pp3=PP(3,1);            % polo planta
% zz1=abs(ZZ(1,1));       % cero planta
% zz2=abs(ZZ(2,1));       % cero planta
% 
% figure; pzmap(Gpd);
% hold on; pzmap((z1),(z2))
% 
% % Proyecto Compensador PD discreto
% % Condicion de angulo
% % Vectores de los polos y ceros a z1
% v1=(pp1-real(z1))+1i*imag(z1);      %fi1
% v2=(real(z1)-pp2)+1i*imag(z1);      %fi2
% v3=(real(z1)-pp3)+1i*imag(z1);      %fi3 este se cancela con el cero del compensador
% v7=(real(z1))+1i*imag(z1);
% 
% % Vector del cero del compensador a z1
% %v3=(real(z1)-a) + j*imag(z1);      %tita_a: cero del compensador
% 
% % Vectores de los ceros a z1
% v4=(real(z1)+zz1)+1i*imag(z1);      %tita1
% v5=(real(z1)+zz2)+1i*imag(z1);      %tita2
% 
% % Angulos de los polos y ceros a z1
% tita1=angle(v4); tita1g=tita1*180/pi;
% tita2=angle(v5); tita2g=tita2*180/pi;
% fi1=pi-angle(v1); fi1g=fi1*180/pi;
% fi2=angle(v2); fi2g=fi2*180/pi;
% fi3=angle(v3); fi3g=fi3*180/pi;
% fi7=angle(v7); fi7g=fi7*180/pi;
% 
% % Obtención del parametro "a" a partir de la condición de angulo
% % fi_a=pi-arctg[imag(z1)/(real(z1) - b)];
% alfa=pi - tita1 - tita2 + (fi1 + fi2 + fi3 + fi7);
% 
% a=((real(z1)*tan(alfa))-imag(z1))/tan(alfa);
% v6=(real(z1) - a) + 1i*imag(z1);
% fi_a=angle(v6); fi_ag=fi_a*180/pi;
% 
% % Condicion de modulo
% VAbs=0.0009953*abs(v4)*abs(v5)*abs(v6)/(abs(v1)*abs(v2)*abs(v3)*abs(v7));
% K=1/VAbs;
% 
% Gc=K*(z - a)/z;
% Gla=Gpd*Gc;
% syslcd_c=feedback(Gla,1);
% figure; step(syslcd_c);
% hold on;
% step(syslcd_nc);
% legend('compensado','no compensado');
% 
% a=pp2;
% v6=(real(z1) - a) + 1i*imag(z1);
% % Condicion de modulo
% VAbs=0.0009953*abs(v4)*abs(v5)*abs(v6)/(abs(v1)*abs(v2)*abs(v3)*abs(v7));
% K=1/VAbs;
% 
% Gcd2=K*(z-a)/z;      % PD
% % FT lazo abierto
% Gla2=Gpd*Gcd2;
% figure; pzmap(Gla2);
% 
% syslcd_c2=feedback(Gla2,1);
% 
% % t=0:T:15;
% figure; step(syslcd_c2);
% hold on
% step(syslcd_nc);
% legend('compensado','no compensado');
%  
% figure; nyquist(Gpd); 
% title('sistema sin compensación')
% figure; nyquist(Glad); 
% title('sistema con compensación')
% 
% figure; margin(Gpd); grid;
% legend('sistema sin compensación')
% figure; margin(Glad); grid;
% legend('sistema con compensación')
% 
% figure; bode(syslc_nc); hold on;
% bode(syslc_c); grid; legend('no compensado','compensado');
% 
% rampa=t;
% figure; yr=lsim(syslc_c,rampa,t); plot(t,rampa,t,yr); grid
% legend('respuesta entrada en rampa')
% 
% error=(rampa-yr')*100; figure; plot(t,error,'k'); grid
% legend('error porcentual entrada en rampa')
% 
% % input('pulse enter para terminar'); clc;