clc; 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)

figure; step(Gplc);
close all

% Diseño del compensador PID: Gc=Kc*(s+a)*(s+b)/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=2.1; wd=2.5;
 
s1d=-sigma+1i*wd;         % Polos dominantes de lazo cerrado
s2d=-sigma-1i*wd;         % complejos conjugados en el dominio continuo

% pds=-sigma+1i*wd;         % Punto deseado

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

% Proyecto Compensador PI: ;
% Condicion de fase
% Vectores de los polos a s1d
v1=s1d;                 %Vector del polo al origen a sd1
v2=s1d + pp2;           %Vector del polo en -2.1 a sd1
v3=s1d + pp3;           %Vector del polo en -2.5 a sd1

% 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
tita_c = -180 + (fi1g + fi3g);

% El compensador debe aportar una fase de aproximadamente 20.23° para que s1d pertenezca 
% al LGR del sistema en LC: tita_c = atan(imag(s1d)/(zpi - real(s1d)))

%% Forma alternativa
angulo_cero=180 - angle(evalfr(Gp/s,s1d))*180/pi - fi2g;

% Cero restante del compensador PID
b = abs(real(s1d)) + (imag(s1d)/tand(tita_c));
% O también:
cero=sigma + wd/tand(angulo_cero);

% Vector del cero del PID al punto s1d
v4=s1d + b;

%% Se obtiene el valor de ganancia del controlador para garantizar el essv
%% mediante la expresión de Kv = Kc*50*(s + b)/(s + 3)
Kv=5;
Kc=5*3/(50*b);

a=pp2;
Gc=Kc*(s + a)*(s + b)/s;
 
Gla=Gc*Gp;
K=0:0.001:100;
figure; rlocus(Gla)
legend('LGR PID')

Glcc1=feedback(Gla,1);

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

figure; pzmap(Glcc1)
title('Polos y Ceros LC compensado con PID');

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

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

%% Diseño de los dispositivos electronicos para la implementación
[nc,dc]=tfdata(Gc,'v');
% Esta es una primera forma
Kd=nc(1); Kp=nc(2); Ki=nc(3);
Ti=Kp/Ki; Td=Kd/Kp;

% Seleccionamos C1 y C2
C1=2.2e-6; C2=4.7e-6;
R1=1/(Ki*C2);
R2=Kd/C1;
R1a=390000; R2a=18000;
Kpr=(R1a*C1 + R2a*C2)/(R1a*C2);

%% Recalculo para no variar Kc significativamente: Se parte de la expresion de Gc en funcion 
%% de los ceros y Kc
% Kc=(R1*C1)*(R2*C2)/(R1*C2)
% tau_a=R1*C1; tau_b=R2*C2;
tau_a=1/a; tau_b=1/b;
% Seleccionamos C2
C2=4.7e-6;
% De la relacion de Kc obtenemos R1
R1=tau_a*tau_b/(Kc*C2);
% De la relacion de tau_a obtenemos C1
C1=tau_a/R1;
% Y de la relacion de tau_b se obtiene R2
R2=tau_b/C2;
% Luego de la prueba con los valores exactos, adoptamos valores comerciales
R1a=390e3; R2a=22e3; C1a=1.5e-6;

tau_ar=R1a*C1a; tau_br=R2a*C2;
Kcr=(R1a*C1a)*(R2a*C2)/(R1a*C2);

close all
%% Cancelacion de ambos polos de la planta
% Condicion de Magnitud
% Se impone un polo real en -5 rad/s
v1=-5+1i*0;
Kc = abs(v1)/Ks;
% Kc=4
a=pp2; b1=pp3;
Gc=Kc*(s + a)*(s + b1)/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');

%% Diseño de los dispositivos electronicos para la implementación
[nc,dc]=tfdata(Gc,'v');
% Kc=(R1*C1)*(R2*C2)/(R1*C2)
% tau_a=R1*C1; tau_b=R2*C2;
tau_a=1/a; tau_b=1/b1;
% Seleccionamos C2
C2=4.7e-6;
% De la relacion de Kc obtenemos R1
R1=tau_a*tau_b/(Kc*C2);
% De la relacion de tau_a obtenemos C1
C1=tau_a/R1;
% Y de la relacion de tau_b se obtiene R2
R2=tau_b/C2;
% Luego de la prueba con los valores exactos, adoptamos valores comerciales
R1a=330e3; R2a=68e3; C1a=1.5e-6;

tau_ar=R1a*C1a; tau_br=R2a*C2;
Kcr=(R1a*C1a)*(R2a*C2)/(R1a*C2);

close all
%% Aumento significativo de la ganancia de Gla 
% Del lugar de raices de la primera solucion elijo una ganancia elevada
Kc=5.7;
Gc=Kc*(s + a)*(s + b)/s;
 
Glar=Gc*Gp;
figure; rlocus(Glar)
Glccr=feedback(Glar,1);

t=0:0.001:4;
figure; step(Glcc1,Glccr,t); 
legend('LC compensado con PID 1er caso','LC compensado con PID Kc elevado');

figure; pzmap(Glccr)
title('Polos y Ceros LC compensado con PID Kc elevado');

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

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

%% Diseño de los dispositivos electronicos para la implementación
% Kc=(R1*C1)*(R2*C2)/(R1*C2)
% tau_a=R1*C1; tau_b=R2*C2;
tau_a=1/a; tau_b=1/b;
% Seleccionamos C2
C2=100e-9;
% De la relacion de Kc obtenemos R1
R1=tau_a*tau_b/(Kc*C2);
% De la relacion de tau_a obtenemos C1
C1=tau_a/R1;
% Y de la relacion de tau_b se obtiene R2
R2=tau_b/C2;

% Luego de la prueba con los valores exactos, adoptamos valores comerciales
R1a=100e3; R2a=1e6; C1a=4.7e-6;

tau_ar=R1a*C1a; tau_br=R2a*C2;
Kcr=(R1a*C1a)*(R2a*C2)/(R1a*C2);