% DISEÑO DE CONTROLADORES PID EN EL DOMINIO DE LA FRECUENCIA - Tema 10 %
clear all
close all
clc

% C(s)=Kc*(1+Td*s+1/(Ti*s))=Kc*(TiTd*s^2+Ti*s+1/Ti*s)
% 3 parametros de diseño: Kc, Ti, Td

%% Caso 1: PI en serie con PD

% C(s)=K * ((1+T1*s)/(T1*s)) * (1+T2*s)
%
% Kc=K*(T1+T2)/T1 
% Ti= T1+T2
% Td=(T1*T2)/(T1+T2)

k=0.5;
a=3.33;
b=0.167;

G=tf(k,[a b 0])
[Gm,Pm,Wgm,Wpm] = margin(G)

figure(1)
bode(G)
grid on

% especificaciones en el tiempo
SO=0.1; ts=5; ev=0.01;

% con un PID agregamos un integrador: el sistema se hace de tipo 2, con lo
% cual ev se cumple.

d = sqrt(log(SO)^2/(pi^2+log(SO)^2))
alpha=acos(d)

wn = (pi-alpha)/(ts*sqrt(1-d^2))

raiz_doble=sqrt(sqrt(1+4*d^4)-2*d^2);

Mf_rad=atan((2*d)/raiz_doble)  % approximado Mf=70-SO
Mf=Mf_rad*180/pi

wcd=wn*raiz_doble % approximado wc=pi/(2*ts)

% 1. Elegir wc y calcular DeltaMF=Mfd-Mf(wc)+10. Tomamos wc > de la que
% definen las especificaciones

wc=0.4

Mf_wc= 7 % -180-Fase(wc) lo obtengo desde el bode

Delta_Mf= Mf - Mf_wc + 10

% 2. Calcular T2 tal que wc*T2=tan(DeltaMF)

Delta_Mf_rad=Delta_Mf*pi/180

T2=tan(Delta_Mf_rad)/wc

% 3. Calcular K tal que |K*PD(wc)*G(wc))|_db=0, es decir |K*PD(wc)*G(wc))|=1
%
% K=1/|PD(wc)*G(wc))|

PD=tf([T2 1],1)

PD_G=PD*G

syms s

PD_Gs=  (2.311*s + 0.5)/ (3.33 * s^2 + 0.167 * s)

s=i*wc;

PD_G_wc=eval(PD_Gs)

K=1/abs(PD_G_wc)
K_db=real2deb(K)

% 4. Elegir T1 tal que 1/T1 \in [wc/30,wc/10]

T1=10/wc

% Armamos el PID

Kc=K*(T1+T2)/T1 
Ti= T1+T2
Td=(T1*T2)/(T1+T2)

C=tf(Kc*[Ti*Td Ti 1],[Ti 0])

G_PID=C*G
[Gm,Pm,Wgm,Wpm] = margin(G_PID)

figure(1)
hold on
bode(G_PID)

Glc=feedback(G_PID,1)
figure(2)
hold on
step(Glc)
datos = stepinfo(Glc,'RiseTimeLimits',[0 1]);

fprintf('ts = %.4f \nSO = %.4f \n', datos.RiseTime, datos.Overshoot)

% Si SO es mayora que la deseada para reajustar se toma un SO un poco
% menor (Mf deseado mayor, por ejemplo Mf=70)

%% Caso 2: K está fijado por las especificaciones en permanente
clear all
close all
clc

% El PID se expresa como 1 integrador y 2 PD
%
% C(s)=K*(1/s)*(1+T1s)*(1+T2s)
%
% Kc=K*(T1+T2)
% Ti= T1+T2
% Td=(T1*T2)/(T1+T2)

num=[1];
den=conv([1 1 0],[1 10])

G=tf(num,den)
[Gm,Pm,Wgm,Wpm] = margin(G)

figure(1)
bode(G)
grid on

%Especificaciones: ev<1% y SO<20
SO=0.4; ea=0.5;

d = sqrt(log(SO)^2/(pi^2+log(SO)^2))
alpha=acos(d)

% wn = (pi-alpha)/(ts*sqrt(1-d^2))

raiz_doble=sqrt(sqrt(1+4*d^4)-2*d^2);

Mf_rad=atan((2*d)/raiz_doble)  % approximado Mf=70-SO
Mfd=Mf_rad*180/pi

% 1. Elegir K que satisfaga las espcecificaciones en permanente

% Ka es limite cuando s->0 de s^2C(s)G(s)
% ea=0.5 ---> 1/Ka=0.5 ---> Ka=K*Kg y Ka=1/0.5

Kg=0.1;
Ka=1/0.5;
K=Ka/Kg

K_db=real2deb(K)


% 2. Estudiar el Bode de G'(s)=(K/s)*G(s)

I=tf([K],[1 0])

G_I=I*G
[Gm,Pm,Wgm,Wpm] = margin(G_I)

figure(1)
hold on
bode(G_I)

% 3. Diseñar los PD para cumplir las especificaciones en el transitorio 
%
% Delta_Mf= Mfd-Mf(sistema compensado)+20
%
% se toman T1=T2, con lo cual cada PD debe aportar la mitad del Delta_Mf

Delta_Mf= Mfd-Pm+20

Delta_Mf_medio=Delta_Mf/2

Delta_Mf_medio_rad=Delta_Mf_medio*pi/180

T1= tan(Delta_Mf_medio_rad)/Wpm % Wpm es la frecuencia de corte, en donde medimos Pm

T2=T1

% 4. Calcular el controldor 

Kc=K*(T1+T2)
Ti= T1+T2
Td=(T1*T2)/(T1+T2)

C=tf(Kc*[Ti*Td Ti 1],[Ti 0])

G_PID=C*G
[Gm,Pm,Wgm,Wpm] = margin(G_PID)

figure(1)
hold on
bode(G_PID)

Glc=feedback(G_PID,1)
figure(2)
hold on
step(Glc)
datos = stepinfo(Glc,'RiseTimeLimits',[0 1]);

fprintf('ts = %.4f \nSO = %.4f \n', datos.RiseTime, datos.Overshoot)


%% funciones que necesito
function b=real2deb(a)

b=20*log10(a);
end

function b=deb2real(a)

b= 10^(a/20);

end