% ------------------ 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));

%% Diseño de 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;

%% Rediseño digital. Se selecciona el periodo T en base al tiempo de subida
%% del sistema compensado. tr = 0.24 seg. Debido a que el sistema posee muy
%% baja estabilidad relativa, se toman 20 muestras x periodo tr 
Nr=20; T=tr/Nr; 
T=0.012; fm=1/T;
z=tf('z',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
%% La FT del compensador PD aproximado por Backward es la siguiente:
Kc=(Kp*T + Kd)/T;  zPD=Kd/(Kp*T + Kd);
Gcd=Kc*(z - zPD)/z;

%% Otra forma es recordando que la aproximación de Backward es la siguiente:
sBW=(z-1)/(z*T);
GcBW=minreal(Kp + Kd*sBW);       % Backward

% FTLA compensada en tiempo discreto
Glad1=minreal(series(Gcd,Gpd));
Glad2=minreal(series(GcBW,Gpd));

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

figure; step(Glcd1,Glcd2,Glc)
title('Respuestas del Sistema en TC y TD')
legend('LC Compensado TD1','LC Compensado TD2','LC Compensado TC')

specd=stepinfo(Glcd1);
trd=specd.RiseTime;
tsd=specd.SettlingTime;
Mpd=specd.Overshoot;

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

%% 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;
    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