clear
close all
clc

s=tf('s');
t=0:0.0001:7;
r=t;

% Planta
Gp=80/(s*(s+2));
[Km,Pp,Zp]=tfdata(Gp,'v');
% figure; step(Gp);
% legend('Planta LA')

Gplc=feedback(Gp,1);
figure; step(Gplc);
legend('Planta LC')
zpk(Gplc)

% Especificaciones
xita=0.75; tr=1.0; wn=1.8/tr;
Glcd=wn^2/(s^2 + 2*xita*wn*s + wn^2);
% figure; step(Glcd)
polos=pole(Glcd);
sigma=abs(real(polos(1)));
wd=imag(polos(1));

Mp1=100*exp(-pi*xita/(sqrt(1 - xita^2)));
Mp2=100*exp(-pi*sigma/wd);

Kd=(2*xita*wn - Pp(2))/Km(3);  Kp=wn^2/Km(3);
zPD=Kp/Kd;

% Gc=Kd*(s + Kp/Kd);

Td=Kd/Kp;

Gc=Kp + s*Kp*Td;
zpk(Gc)

Gla=Gc*Gp;
Glc=feedback(Gla,1);
zpk(Glc)
[Wlc,Zlc]=damp(Glc)

figure; step(Glc,Gplc,t)
legend('Planta LC compensada','Planta LC no compensada')

figure; step(Glc,Glcd,t)
legend('Planta LC compensada','Planta LC deseada')

figure; pzmap(Gplc,Glc)
legend('polos y ceros LC sin PD','polos y ceros LC con PD')
%% Acción de control
% [y,t]=step(Glc,t);
% ref=ones(length(t),1);
% error=ref - y;
% figure; lsim(Gc,error,t)

close all
%% Reajuste compensador

Kd=0.012014;  cero_PD=3.371;

Td=Kd/Kp;
Kp=Kd*cero_PD;

Gc=Kd*(s + cero_PD);
Gla=Gc*Gp;
Glc=feedback(Gla,1);
zpk(Glc)
[Wlc,Zlc]=damp(Glc)

figure; step(Glc,Gplc,t)
legend('Planta LC compensada','Planta LC no compensada')

figure; step(Glc,Glcd,t)
legend('Planta LC compensada','Planta LC deseada')

figure; pzmap(Gplc,Glc)
legend('polos y ceros LC sin PD','polos y ceros LC con PD')

%% Acción de control
% [y,t]=step(Glc,t);
% error=ref - y;
% figure; lsim(Gc,error,t)