clear
close all

s=tf('s');
% Planta

Gp=50/((s+2)*(s+3));
[Z,P,Ks] = zpkdata(Gp,'v');

Gplc=feedback(Gp,1);
figure; rlocus(Gp)
figure; margin(Gp)

t=0:0.001:10;
figure; step(Gplc);

% Exigencia de essv=20%. Kv=Ks*Ki/(2*3)
Kv=5;

Ki=Kv*2*3/Ks;
Glai=(Ki/s)*Gp;
Glci=feedback(Glai,1);
figure; rlocus(Glai)

r=t;
figure; step(Glci,t);

[y]=lsim(Glci,r,t); 
essv=(r'-y);
figure; plot(t,r,t,y,t,essv)
legend('Referencia','Salida','essv');

% Se mantiene la misma ganancia Ki para no afectar el essv y se calcula
% el cero de un PI para cumplir el ts = 2 seg
sigma=4/2;
cero_PI=sigma; Kc=Ki;
Gc1=Kc*(s + cero_PI)/s;

Gla1=Gc1*Gp;
figure; rlocus(Gla1)
Glc1=feedback(Gla1,1);
figure; step(Glc1);

t=0:0.001:5;
r=t;
[y]=lsim(Glc1,r,t); 
essv=(r'-y);
figure; plot(t,r,t,y,t,essv)
legend('Referencia','Salida','essv');

close all

% Diseño del compensador PI: Gc = Kpi*(s + zpi)/s
% Especificaciones de desempeño
Mp=15/100;              % Sobrepaso maximo del 15%. Mp=exp(-pi.sigma/wd)
ts=2;                   % Tiempo de establecimiento 2seg. 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

% sigma_m=2.1; wd_m=3;
% 
% 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 dominio continuo

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

% Diseño Compensador PI
% 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 -2 a sd1
% v3=((real(s1d)+abs(pp3)) + 1i*imag(s1d));      %Vector del polo en -3 a sd1

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

% Angulos de los polos a s1d
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 104° para que s1d pertenezca al LR del sistema en LC
% tita_c = atan(imag(s1d)/(zpi - real(s1d)))
tita_c = fase_total;
alfa=180-tita_c;   % alfa es el angulo complementario que forma el vector del zpi con el eje real (-)

% zpi = abs(real(s1d)) + (imag(s1d)/tand(tita_c));
zpi = abs(real(s1d)) - (imag(s1d)/tand(alfa));

% Vector del cero del PI al punto s1d
v4=s1d + zpi;
% titapi=angle(v4); titapig=titapi*180/pi;

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

Gc=Kc*(s + zpi)/s;

Gla=Gc*Gp;
figure; rlocus(Gla)
Glcc=feedback(Gla,1);

t=0:0.001:4;
figure; step(Gplc,Glcc,t); 
legend('LC sin compensacion','LC compensado con PI');

figure; pzmap(Glcc)
title('Polos y Ceros LC compensado con PI');

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

r=t;
[y]=lsim(Glcc,r,t); 
essv=(r'-y);
figure; plot(t,r,t,y,t,essv)
legend('Referencia','Salida','essv');

% Error de velocidad
Kv=Kc*zpi*Ks/(2*3);
essv=(1/Kv)*100;

%% Diseño de los dispositivos electrónicos para la implementación
% Kp=R2/R1  Td=R1*C1
Ti=1/zpi; Kp=Kc; Ki=zpi*Kp;
% Seleccionamos C2=1uF
C2=4.7e-6; R2=Ti/C2;
R2a=180000;
R1=R2/Kp;
R1a=820000;

close all
%% PI Reajustado
Kc=0.1954; zpi=1.532;
Gc=Kc*(s + zpi)/s;

Gla=Gc*Gp;
figure; rlocus(Gla)
Glcc=feedback(Gla,1);

t=0:0.001:10;
figure; step(Gplc,Glcc,t); 
legend('LC sin compensacion','LC compensado con PI');

figure; pzmap(Glcc)
title('Polos y Ceros LC compensado con PI');

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

%% Diseño de los dispositivos electrónicos para la implementación
Ti=1/zpi; Kp=Kc; Ki=zpi*Kp;
% Seleccionamos C2=1uF
C2=4.7e-6; R2=Ti/C2;
R2a=150000;
R1=R2/Kp;
R1a=820000;

% sisotool(Gp,Gc,1,1)