clear
close all
clc

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

% Planta
Gp=0.5/((s+0.1)*(s+0.5));
[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.8; tr=2.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);

sigma_nd=10*sigma;
% Pcd=(s+sigma_nd)*(s+sigma+1i*wd)*(s+sigma-1i*wd);
Pcd=s^3 + s^2*(2*sigma + sigma_nd) + s*(sigma^2 + 2*sigma_nd*sigma + wd^2) + (sigma_nd*sigma^2 + sigma_nd*wd^2);
zero(Pcd)

z1=0.8; z2=1.2843; Kp=8;
Gc=Kp*(s+z1)+(s+z2)/s;

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

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

close all

figure; step(Glc,Glcd,t)
legend('Planta LC compensada con PID','Planta Deseada')

%% Reajuste PID
 
%       42.843 (s^2 + 0.2244s + 0.1597)
% C =   -------------------------------
%                     s

% Gcr=42.843*(s^2 + 0.2244*s + 0.1597)/s;
% Glar=Gcr*Gp;
% figure; rlocus(Glar)
% 
% Glcr=feedback(Glar,1);
% zpk(Glcr)
% [Wlc,Zlc]=damp(Glcr)
% 
% figure; step(Glcr,Gplc,t)
% legend('Planta LC compensada','Planta LC no compensada')
% 
% % Entrada en rampa
% t=0:0.0001:70;
% r=t;
% [y,t]=lsim(Glcr,r,t);
% e_rampa=r' - y;
% 
% figure; plot(t,r,t,y,t,e_rampa) 
% figure; plot(t,e_rampa)