% ------------------ SISTEMAS DE CONTROL 2---- --------------- 
% Ejemplo 1: Método de Rediseño Digital.
% Controlador Proporcional-Derivativo.
% ------------------------------------------------------------
clc; clear; close all;
format long

s=tf('s');

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

%% Se diseña un compensador PD para que se cumplan las siguientes especificaciones:
%% Mp menor o igual al 10%, ts menor o igual a 1 segundo y essp = 0
%% El compensador diseñado por el LGR en el dominio del tiempo continuo resulta: 
Kp=0.6593; Kd=0.1;
Gc=Kp + Kd*s;
% FTLC sin compensar en tiempo continuo
Glcsc=minreal(feedback(Gp,1));
% FTLA compensada en tiempo continuo
Gla=minreal(series(Gc,Gp));
% FTLC compensada en tiempo continuo
Glc=minreal(feedback(Gla,1));

figure; step(Glc,Glcsc)
title('Respuestas del Sistema en Tiempo Continuo')
legend('LC Compensado','LC Sin Compensar')

spec=stepinfo(Glc);
tr=spec.RiseTime;
ts=spec.SettlingTime;

%% Rediseño digital. 
%% Se selecciona el periodo T en base al tiempo de subida
%% del sistema compensado tr = 0.24 s y se toman inicialmente 20 muestras 
Nr=20; Tm=tr/Nr;
% Se adopta el siguiente valor de T
% T=0.024; fm=1/T;

%% Otra opción es seleccionar el periodo T en base al periodo de oscilación Td
%% de los polos dominantes del sistema compensado Glc
%% Se toman inicialmente 20 muestras
% Se calculan los polos dominantes de Glcr para obtener la parte imaginaria wd 
Polos_LC=pole(Glc);
wd_des=imag(Polos_LC(1));
% A partir del valor de wd deseado obtenemos el periodo de muestreo 
Td=2*pi/wd_des; 
%% Se observa que la relación entre Td y tr es mayor a 5 veces, por lo que
%% esta debe ser la relación aproximada entre Nd y Nr, según el tiempo que se utilice
Nd=5*Nr;
Tm=Td/Nd; 

if Nd==50
    T=0.025;
end
if Nd==100
    T=0.012;
end

z=tf('z',T);
td=0:T:2-T;
%% Puede verificarse el periodo T trazando las respuestas de LCSC en TC y TD
%% Se utilizará la aproximación invariante al escalón para obtener la FT muestreada de Gp
Gpd=c2d(Gp,T,'zoh');
% FTLC sin compensar en tiempo discreto
Glcscd=minreal(feedback(Gpd,1));
figure; step(Glcsc,Glcscd)
title('Verificar el periodo de muestreo T')
legend('LC Sin Compensar TC','LC Sin Compensar TD')

%% A continuación se pasa a la etapa de rediseño digital
%% Se obtiene la FT aproximada del compensador PD por Backward:
Kc=(Kp*T + Kd)/T;  zPD=Kd/(Kp*T + Kd);
GcdBW1=Kc*(z - zPD)/z;

%% Otra forma es recordando que la aproximación Backward es la siguiente:
sBW=(z-1)/(z*T);
GcdBW2=minreal(Kp + Kd*sBW);
%% Es fácil observar que ambas expresiones resultan iguales

%% Se obtiene la FT aproximada por Tustin 
sT=(2/T)*((z-1)/(z+1));
GcdTustin=minreal(Kp + Kd*sT);
 
%% Validación del rediseño
% FTLA compensada en tiempo discreto
Glad1=minreal(series(GcdBW2,Gpd));
Glad2=minreal(series(GcdTustin,Gpd));

% FTLC compensada en tiempo discreto
Glcd1=minreal(feedback(Glad1,1));
Glcd2=minreal(feedback(Glad2,1));

figure; step(Glc,Glcd1,Glcd2,td)
title('Respuestas del Sistema en TC y TD')
legend('LC en TC','LC en TD Backward','LC en TD Tustin')

%% Acción de control digital 
figure; step(feedback(GcdBW2,Gpd)); legend('Acción de Control PD Backward')
figure; step(feedback(GcdTustin,Gpd)); legend('Acción de Control PD Tustin')

%% Estabilidad en tiempo discreto
%% Estabilidad 
figure; bode(Gc*Gp,GcdBW2*Gpd,GcdTustin*Gpd); grid
legend('SC TC','SC TD con BackWard','SC TD con Tustin');

close all
%% Simulación Digital
num=1080; den=[1 24 108 0];
[A,B,Cr,D]=tf2ss(num,den);
[G,H]=c2d(A,B,T);
C=[0 0 1];
tf=2;                   % Tiempo total de simulación
np=round(tf/T);         % Número de puntos de la simulación
% 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(3,np);

for k=2:np
    t(k)=(k-2)*T;
    r(k)=1.0;
    y(k)=C*x(:,k);
    e(k)=r(k)-y(k);
    u(k)=Kc*e(k) - zPD*Kc*e(k-1);
    x(:,k+1)=G*x(:,k) + H*1080*u(k);
end

figure; step(Glc); hold on
stairs(t(1:k),y(1:k),'k');
plot(t(1:k),y(1:k),'.r'); 
grid;
title('Respuesta de la Posición Angular del Motor CC')
legend('Salida Sistema Continuo','Salida Sistema Discreto con ZOH','Salida Sistema Digital');
% axis([0 max(t) 0 1.2])

figure;
stairs(t(1:k),u(1:k)); grid
legend('acción de control PD');
axis([0 0.6 -1 max(u)+0.5])

figure;
stairs(t(1:k),x(2,1:k))
legend('Velocidad angular'); grid

figure;
stairs(t(1:k),x(1,1:k))
legend('Corriente'); grid

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