clear
close all

s=tf('s');

% Se plantea el control de posición angular del eje de un motor CC
Gpc=1080/(s*(s+6)*(s+18));
paso_sim=8e-6;

% Se define vector de tiempo continuo para simulación
tc=0:paso_sim:4-paso_sim;
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.05;  % 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.4;
z=tf('z',T);
Gpd=c2d(Gpc,T);

% Discrete-time transfer function:

%      2.004 z^2 + 1.606 z + 0.02375
% ---------------------------------------
% z^3 - 1.091 z^2 + 0.09153 z - 6.773e-05
%  
% Sample time: 0.4 seconds

num=1080;
den=[1 24 108 0];

% Los estados de la planta son: x1 = Posición; x2 = Velocidad y x3 = Corriente armadura
A=[0      1      0
   0      0      1
   0   -108    -24];
% Matriz de entrada que vincula las entradas con los estados del proceso a controlar
B=[0;0;1080];
% Matriz de salida. En este caso la salida es igual al estado x1, o sea, la posición angular.
C=[1  0  0];
Ds=0;

F=Gpd.num; D=Gpd.den;
n=F{:,:}; d=D{:,:};
b1=n(2); b2=n(3); b3=n(4);
a1=d(2); a2=d(3); a3=d(4);

q0=1/(b1 + b2 + b3); q1=q0*a1; q2=q0*a2; q3=q0*a3;
p1=q0*b1; p2=q0*b2; p3=q0*b3;

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

Gcd=minreal(Qz/(1 - Pz));
% Se impone una referencia de 90° o pi/2 radianes
td=0:T:4.4-T;
Glcd=Pz; step((pi/2)*Glcd,'r',td); hold on; step((pi/2)*Glcd,'bo',td); grid; %axis([0 1.2 0 1.2]); 

% Variación paramétrica utilizando FT
Gpc_vp=1080/(s^3 + 22*s^2 + 98*s);
Gpd_vp=c2d(Gpc_vp,T);
Glad_vp=series(Gcd,Gpd_vp);
Glcd_vp=minreal(feedback(Glad_vp,1));
figure;
step((pi/2)*Glcd_vp,'r',td); hold on; step((pi/2)*Glcd_vp,'bo',td); grid; %axis([0 1.2 0 1.2]); 

td=0:T:5.6-T;
[yz,Td]=step((pi/2)*Glcd,td);

%% Se redefinen las matrices A y B para simular la variación paramétrica.
% Si no se desea variación paramétrica deben comentarse estas matrices
flag_var_par = 0;

if flag_var_par == 1
    
    A=[0      1      0
       0      0      1
       0    -98    -22];
    
    B=[0;0;1080];
end

% Obtención de las matrices de estado en tiempo discreto para la simulación
[G,H]=c2d(A,B,T);

% Inicialización de variables
m=length(Td);
y=zeros(1,m);
r=zeros(1,m);
e=zeros(1,m);
t=zeros(1,m);
u=zeros(1,m);
x=zeros(3,m);

for k=4:m
    t(k)=(k-4)*T;
    r(k)=pi/2;
    y(k)=C*x(:,k);
    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);
    x(:,k+1)=G*x(:,k) + H*u(k);
end

figure; stairs(t,y,'b'); hold on; plot(t,y,'ko'); grid; axis([0 max(t) 0 1.7]);
legend('Salida de la Planta')
figure; stairs(t,e,'r'); hold on; plot(t,e,'ko'); grid; axis([0 max(t) 0 1.7]);
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')

figure; stairs(t,x(1,1:k)); hold on; plot(t,x(1,1:k),'ro'); grid; 
legend('Estado x1 = Posición')

figure; stairs(t,x(2,1:k)); hold on; plot(t,x(2,1:k),'ro'); grid; 
legend('Estado x2 = Velocidad')

figure; stairs(t,x(3,1:k)); hold on; plot(t,x(3,1:k),'ro'); grid; 
legend('Estado x3 = Corriente')