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


num=[1 5];
den=conv([1 0.1],[1 2 2])

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

figure(1)
bode(G)
grid on

%Especificaciones: ep<1% y SO<20
SO=0.2; ep=0.01;

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

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

% Kp es limite cuando s->0 de C(s)G(s)
% ep=0.01 ---> 1/(1+Kp)=0.01 ---> Kp=Kc*Kg y Kp=99 y Kg=25

Kc=99/25

Kc=4

Kcdb=real2deb(Kc)

G_P=Kc*G
[Gm,Pm,Wgm,Wpm] = margin(G_P)

figure(1)
hold on
bode(G_P)

% En le Bode vemos que Mf=-15º pero necesitamos almenos 50, entonces
% tenemos que sumar fase. Agregamos un cero. La posición del cero depende
% de la fase que queramos sumar

Delta_Mf=Mf-Pm

if Delta_Mf<45 %en este caso 1/Td > wc
    Delta_Mf_rad=(Delta_Mf*pi)/180
    Td=atan(Delta_Mf_rad/Wpm)
else           % Si Delta_Mf>=45 entonces 1/Td <= wc
    
    Delta_Mf=Mf-Pm+10
    Delta_Mf_rad=(Delta_Mf*pi)/180
    Td=tan(Delta_Mf_rad)/Wpm
end

C=tf([Td 1],1)

Gcomp=C*G_P

[Gm,Pm,Wgm,Wpm] = margin(Gcomp)

figure(1)
hold on
bode(Gcomp)

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

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

%% Caso 2: Kc no está fijada por las especificaciones
% Paso 1: elegir wc en función de  las especificacioens
% Paso 2: elegir Td para satisfacer las condiciones de Mf
% Paso 3: Ajustas Kc para garantizar el valor de wc

clear all
close all
clc


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

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

figure(1)
bode(G)
grid on

%Especificaciones: ev<1% y SO<20
SO=0.2; ev=0.01;

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

% Kv es limite cuando s->0 de sC(s)G(s). Como el sistema es de tipo 2,
% tengo error en velocidad nulo siempre.
% por lo tanto Kc no está dada por las especificaciones.
% Vamos a fijar wc en función del margen de fase.

% opción 1
Delta_Mf=Mf-Pm

if Delta_Mf<45 %en este caso 1/Td > wc
    Delta_Mf_rad=(Delta_Mf*pi)/180
    Td=atan(Delta_Mf_rad/Wpm)
else           % Si Delta_Mf>=45 entonces 1/Td <= wc
    
    Delta_Mf=Mf-Pm+10
    Delta_Mf_rad=(Delta_Mf*pi)/180
    Td=tan(Delta_Mf_rad)/Wpm
end

C=tf([Td 1],1)

Gcomp=C*G

[Gm,Pm,Wgm,Wpm] = margin(Gcomp)

figure(1)
hold on
bode(Gcomp)

% ahora fijamos Kc, para que wc=Wpm

syms s

Gcomp_w= (13.4*s + 5)/(s^3 + 5*s^2)
 

wc=Wpm;
s=i*wc;
Gcomp_wc=eval(Gcomp_w)

Kc=1/abs(Gcomp_wc)

G_PD=Kc*Gcomp
[Gm,Pm,Wgm,Wpm] = margin(G_PD)


figure(1)
hold on
bode(G_PD)

Glc=feedback(G_PD,1)
figure(2)
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