clear
close all

s=tf('s');
% Planta
Gp=1080/(s*(s+6)*(s+18));
K=0:0.01:200;
figure; rlocus(Gp,K)
syslc=feedback(Gp,1);
figure; step(syslc);

% Diseño compensador PD
% Especificaciones de desempeño
Mp=10/100;              % Sobrepaso maximo del 10%. Mp=exp(-pi.sigma/wd)
ts=1;                   % Tiempo de establecimiento 1seg. ts=4/sigma
sigma=4/ts;             % Parte real de los polos dominantes de lazo cerrado
wd=-pi*sigma/log(Mp);   % Parte imaginaria de los polos dominantes de lazo cerrado

%% EJEMPLO 01
sigma_m=4.2; wd_m=5;

s1d=-sigma_m+1i*wd_m;       % Polos dominantes de lazo cerrado
% s1d=-sigma+1i*wd;       % Polos dominantes de lazo cerrado
s2d=-sigma-1i*wd;       % complejos conjugados en el plano-s

[Z,P,Ks] = zpkdata(Gp,'v');

% Polos y ceros de la planta
PP=pole(Gp);
ZZ=zero(Gp);
pp1=PP(1);
pp2=abs(PP(3));
pp3=abs(PP(2));

% Condicion de fase
% Vectores de los polos a s1d
% v1=real(s1d) + 1i*imag(s1d);                   %Vector del polo al origen a sd1
% v2=((real(s1d)+abs(pp2)) + 1i*imag(s1d));      %Vector del polo en -6 a sd1
% v3=((real(s1d)+abs(pp3)) + 1i*imag(s1d));      %Vector del polo en -18 a sd1

v1=s1d;
v2=s1d + pp2;
v3=s1d + pp3;

% Angulos de los polos a s1
fi1=angle(v1); fi1g=fi1*180/pi;
fi2=angle(v2); fi2g=fi2*180/pi;
fi3=angle(v3); fi3g=fi3*180/pi;

% Condicion de Fase
fase_total = -180 + (fi1g + fi2g + fi3g);

% El compensador debe aportar una fase de aproximadamente 40° para que s1d pertenezca al LR del sistema en LC
% tita_c = atan(imag(s1d)/(zpd-real(s1d)))
tita_c = fase_total;
zpd = abs(real(s1d)) + (imag(s1d)/tand(tita_c));

% Vector del cero del PD al punto s1d
v4=s1d + zpd;

% Condicion de Magnitud
Kd = abs(v1)*abs(v2)*abs(v3)/(Ks*abs(v4));

%% Compensador PD ya diseñado: Mp=10% y ts=1seg
Kp=zpd*Kd;

Gc=Kd*(s + zpd);
% Reajustado
% Kd=0.0661; zpd=10.19; Kp=zpd*Kd;
% Gc=Kd*(s + zpd);

t=0:0.0001:3;

Gla=Gc*Gp;
figure; rlocus(Gla)
Glc=feedback(Gla,1);
figure; step(syslc,Glc); 
legend('LC sin Compensacion','LC Compensado con PD');

r=t;
[y]=lsim(Glc,r,t); 
essv=(r'-y);
figure; plot(t,r,t,y,t,essv)

figure; nyquist(Gp,Gla);
legend('LC sin Compensacion','LC Compensado con PD');

figure; margin(Gla);
legend('Estabilidad Sistema Compensado')

%% Diseño de los dispositivos electrónicos para la implementación
% Kp=R2/R1  Td=R1*C1
Td=1/zpd; 
% Seleccionamos C1=1uF
C1=1e-6; R1=Td/C1;
R1a=100000;
R2=Kp*R1a;
R2a=68000;

close all

%% EJEMPLO 02
% Para el mismo sobreimpulso anterior del 10% pero ts=0.4seg
% los polos dominantes estan en: -10+13.646
Kd=0.2388; a=4.4324; Kp=a*Kd;
Gc=Kp + s*Kd;
Gla=Gc*Gp;
K=0:0.001:200;
figure; rlocus(Gla,K)
sysla=tf(Gla);
syslcc=feedback(sysla,1);
N1=syslcc.num;
D1=syslcc.den;
n1=N1{:,:};
d1=D1{:,:};
raices_lc=roots([d1(1) d1(2) d1(3) d1(4)]);
figure; step(syslcc,syslc); 
legend('Compensado con PD zpd=4,4','No Compensado');

figure; pzmap(syslcc)
legend('Polos-Ceros Sistema Compensado')

[y]=lsim(syslcc,r,t); 
essv=(r'-y);
figure; plot(t,r,t,y,t,essv)

figure; margin(sysla);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Diseño de los dispositivos electrónicos para la implementación
% Kp=R2/R1  Td=R1*C1
Td=1/a; 
% Seleccionamos C1=1uF
C1=1e-6; R1=Td/C1;
R1a=220000;
R2=Kp*R1;
R2a=270000;

Tda=R1a*C1; Kpa=R2a/R1a;