clc; clear
close all

s=tf('s');
% Planta

Gp=50/((s+2)*(s+3));
[Z,P,Ks] = zpkdata(Gp,'v');
figure; rlocus(Gp)
legend('LGR Sistema Sin Compensar') 
figure; margin(Gp)
legend('Estabilidad Relativa Sin Compensar')

%% Verificamos desempeño en LC sin compensar
Gplc=feedback(Gp,1);

t=0:0.0001:15;
figure; step(Gplc);
legend('Respuesta del Sistema a LC Sin Compensar')

%% Especificaciones de desempeño requeridas en LC
%% Mp menor o igual al 5%; ts menor o igual a 2 s; essp = 0 y essv menor al 20%
% El valor de Kv del sistema sin compensar es cero lo que implica un essv
% que tiende a infinito cuanto s tiende a 0. Para llevar a cero el essp y
% obtener un essv finito constante debe al menos incorporarse un polo al
% origen. Se incorpora entonces un controlador integral Gi(s)=Ki/s

% Con esta solución, la Gla(s)=(Ki/s)*Gp(s) y Ki puede diseñarse del LGR de Gla(s)

Gla=Gp/s;
figure; rlocus(Gla)
%% Del LGR la ganancia Ki para un Mp=5% es 0.0725
Ki=0.0725;
Gi=Ki/s;
Glac=Gi*Gp;
Glcc=feedback(Glac,1);

% Se verifica el desempeño con el controlador integral
figure; step(Glcc,t)
legend('Respuesta del Sistema a LC Con Compensador Integral')

figure; margin(Glac)
legend('Estabilidad Relativa Con Compensador Integral')

% Cálculo error de velocidad con acción integral
r=t;
[y]=lsim(Glcc,r,t); 
essv=(r'-y);
figure; plot(t,r,t,y,t,essv)
legend('Referencia','Salida','essv');

close all

%% El Kv = 0.6042 y el essv = 1.655, o sea, un 165% lo cual se debe a la reducida ganancia de LA
%% Para incrementar la ganancia de LA debe hacerse sin comprometer el sobrepaso 
%% y a la vez conseguir reducir el tiempo de asentamiento. Para esto incorporamos 
%% un compensador PI

%% Primera solución: eligiendo el cero del PI mediante la cancelación del polo
%% más dominante de la planta y manteniendo la ganancia diseñada en el punto anterior

t=0:0.0001:8;
cero_PI=abs(P(2)); Kc=Ki;
GcPI=Kc*(s + cero_PI)/s;

GlaPI=minreal(GcPI*Gp);
figure; rlocus(GlaPI)
legend('LGR Sistema Compensado con PI')

figure; margin(GlaPI)
legend('Estabilidad Relativa Con Compensador PI')

GlcPI=feedback(GlaPI,1);
figure; step(GlcPI,t);
legend('Respuesta del Sistema a LC Con Compensador PI')

figure; step(feedback(GcPI,Gp))
legend('Acción de Control PI')

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

close all

%% Con estos resultados se observa que para reducir significativamente el essv hay que
%% aumentar la ganancia de LA lo que conlleva a deteriorar el desempeño transitorio para 
%% entrada en escalón.Se diseñará a continuación el PI por las propiedades del LGR
%% considerando otras especificaciones transitorias

% Diseño del compensador PI: GcPI = 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

% s1d=-sigma+1i*wd;     % Polos dominantes de lazo cerrado
% s2d=-sigma-1i*wd;     % complejos conjugados en el dominio continuo
%% Puedo tomar la parte real del polo deseado un valor mayor que el deseado y la parte imaginaria
%% un poco menor al deseado en el sentido de garantizar que tanto el Mp como el ts sean 
%% menores a los valores deseados.
sigma_m=2.0; wd_m=3.3; 
pds=-sigma_m+1i*wd_m;   % Punto deseado

s1d=sigma_m + 1i*wd_m; s2d=sigma_m - 1i*wd_m;
% FT deseada
Gdes=1/((s + s1d)*(s + s2d));

% Polos y ceros de la FT de LA sin compensar con el polo al origen del PI
% Compensador PI: GcPI = Kp + Ki/s = Kpi*(s + cero_PI)/s; y cero_PI=Ki/Kp
[Z,P,Kla] = zpkdata(Gp/s,'v');
p1=P(1);
p2=abs(P(3));
p3=abs(P(2));
figure; pzplot(Gp/s); hold on
pzplot(Gdes); %axis([-4e4 0.5e4 -2.0e4 2.0e4])
legend('Polos-ceros FT LA SC','Polo deseado s1d')

%% Condición de fase
% Vectores de los polos al punto deseado s1d
v1=real(s1d)+1i*imag(s1d);       %vector del polo al origen del PI a s1d
v2=(p2-real(s1d))+1i*imag(s1d);  %vector del polo p3 a s1d
v3=(p3-real(s1d))+1i*imag(s1d);  %vector del polo p2 a s1d

% Aportes angulares de los polos al punto deseado s1d
fi1=pi-angle(v1);      fi1g=fi1*180/pi;   %fi1g ángulo en grados del polo al origen del PI a s1d
fi2=angle(v2);         fi2g=fi2*180/pi;   %fi2g ángulo en grados del polo p2 a s1d
fi3=angle(v3);         fi3g=fi3*180/pi;   %fi3g ángulo en grados del polo p3 a s1d

%% Condición de fase
% Aporte de fase de los polos de la planta y del polo al origen del PI en el polo deseado
aporte_fi=fi1g + fi2g + fi3g;
tita1=-180 + aporte_fi;        
% Respecto al signo de 180° de la condición de fase cuando K > 0:
% Tomar -180° o +180° es indiferente, ya que en un caso tendremos un ángulo
% menor a 360° y en el otro un ángulo mayor a 360°
% En este caso específico será: 104.36° o 464.36° respectivamente
% Esto significa que el compensador PI debe aportar 104.36° de fase 
% o lo que es lo mismo 464.36° - 360° = 104.36°, para cumplir con la
% condición de fase y hacer que pds pertenezca al LGR.

% Otra forma de calcular el ángulo en una sola expresión es la siguiente
% la cual da como resultado el mismo valor calculado anteriormente de 104.36°
angulo_PI=180-angle(evalfr(Gp/s,pds))*180/pi;

% Cero del compensador PI
cero_PI=real(s1d) + imag(s1d)/tand(tita1);
% O también:
cero=sigma_m + wd_m/tand(angulo_PI);

% Vector del cero del PI al punto s1d
v4=(cero_PI - real(s1d))+1i*imag(s1d);

%% Condicion de magnitud
Kp=(norm(v1)*norm(v2)*norm(v3))/(Kla*norm(v4));

% Otra forma de calcular la ganancia en una sola expresión es la siguiente
% la cual da como resultado el mismo valor calculado anteriormente de 0.1350
Kc=1/abs(evalfr(Gp*(s + cero_PI)/s,pds));

GcPI=Kc*(s + cero_PI)/s;

GlaPI=minreal(GcPI*Gp);
figure; rlocus(GlaPI)
legend('LGR Sistema Compensado con PI LGR')

figure; margin(GlaPI)
legend('Estabilidad Relativa Con Compensador PI LGR')

GlcPI=feedback(GlaPI,1);
figure; step(GlcPI,t);
legend('Respuesta del Sistema a LC Con Compensador PI LGR ')

figure; step(feedback(GcPI,Gp))
legend('Acción de Control PI LGR')

figure; pzplot(GlcPI); 
legend('Polos-ceros LC con PI','Polos deseados')

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

close all
%% Mediante SISOTOOL se realiza un ajuste de la ganancia para cumplir con el ts
%% y manteniendo Mp por debajo del 15%
% sisotool(Gp,GcPI,1,1)
% Luego del ajuste se obtiene:
Kp=0.36272; cero_PI=1.155;
GcPI=Kp*(s + cero_PI)/s;

GlaPI=minreal(GcPI*Gp);
figure; rlocus(GlaPI)
legend('LGR Sistema Compensado con PI LGR')

figure; margin(GlaPI)
legend('Estabilidad Relativa Con Compensador PI LGR')

GlcPI=feedback(GlaPI,1);
figure; step(GlcPI,t);
legend('Respuesta del Sistema a LC Con Compensador PI LGR ')

figure; step(feedback(GcPI,Gp))
legend('Acción de Control PI LGR')

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

%% Diseño de los dispositivos electrónicos para la implementación
% Kp=R2/R1  Ti=R2*C2   Ver presentación
Ti=1/cero_PI; Ki=cero_PI*Kp;
% Seleccionamos C2=1uF
C2=4.7e-6; R2=Ti/C2;
R2a=180000;
R1=R2/Kp;
R1a=470000+33000;