% DISEÑO DE CONTROLADORES CON LUGAR DE LAS RAICES - Tema 9 %
clear all
close all
clc

%% Ejemplo 5 - red de avance

num=3;
d1=conv([1 1],[1 2]);
den=conv([1 6],d1)

G=tf(num,den)

figure(1)
rlocus(G)

% Determinar controlador tal que SO=20% y ts=.5

% Siendo SO=exp(-((pi*d)/sqrt(1-d^2))), entonces 
% d = sqrt(log(SO)^2/(pi^2+log(SO)^2))

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

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

% El sistema en lazo cerrado deberá tener un polo en A= -d*wn + (wn*sqrt(1-d^2))*i

A= -d*wn + (wn*sqrt(1-d^2))*i

figure(1)
hold on
plot(real(A),imag(A),'x')

% modificamos el lugar de las raices colocando un cero. Vamos a tomar c tal
% que c \in [d*wn,wn]

c=3

% para colocar el polo, vamos a usar el criterio del argumento:la suma de los
% angulos que los ceros reales forman con A menos los angulos que los polos
% reales forman con A tiene que dar pi (180º) (ver diapo 68)

arg_Ap= -pi + atan(imag(A)/(real(A)+c)) - atan(imag(A)/(real(A)+1)) - atan(imag(A)/(real(A)+2)) - atan(imag(A)/(real(A)+6))

% arg_Ap=atan(imag(A)/(real(A)+p)) ---> tan(arg_Ap)=imag(A)/(real(A)+p)

p=(imag(A)/tan(arg_Ap))-real(A) % el polo real está en -p

% Creamos la G compensada

C=tf([1 c],[1 p])

G_comp=C*G

figure(2)
hold on
rlocus(G_comp)
plot(-real(c),imag(c),'o')
plot(-real(p),imag(p),'x')
plot(real(A),imag(A),'x')

% para deteminar K usamos el criterio del modulo: K= |1/G_comp(A)|
% para hacerlo, primero construimos la G_comp(s) simbólica

syms s

Gs_comp= (3*s + 9)/(s^4 + 26.47*s^3 + 177.2*s^2 + 361.4*s + 209.7)

s=A;

Gs_comp_A=eval(Gs_comp)

K=1/abs(Gs_comp_A)

% el controlador está dado por
RA=K*C

% ahora dibujamos la respuesta al escalón

G_lc=feedback(RA*G,1)

figure(3)
hold on
rlocus(RA*G)
plot(real(A),imag(A),'x')

figure(5)
hold on
step(G_lc)
datos = stepinfo(G_lc,'RiseTimeLimits',[0 1]);

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


%% si el sistema lc no respeta las especificaciones hay que reajustar: por ejemplo tomar un d mayor (implica bajar la SO)

d2 = 1.1*d
alpha2=acos(d2)

ts=.5;
wn2 = (pi-alpha2)/(ts*sqrt(1-d2^2))

% El sistema en lazo cerrado deberá tener un polo en A= -d*wn + (wn*sqrt(1-d^2))*i

A2= -d2*wn2 + (wn2*sqrt(1-d2^2))*i

c= 3

arg_Ap2= -pi + atan(imag(A2)/(real(A2)+c)) - atan(imag(A2)/(real(A2)+1)) - atan(imag(A2)/(real(A2)+2)) - atan(imag(A2)/(real(A2)+6))

% arg_Ap=atan(imag(A)/(real(A)+p)) ---> tan(arg_Ap)=imag(A)/(real(A)+p)

p2=(imag(A2)/tan(arg_Ap2))-real(A2) % el polo real está en -p

% Creamos la G compensada

C2=tf([1 c],[1 p2])

G_comp2=C2*G

syms s

Gs_comp2= (3*s + 9)/(s^4 + 41.36*s^3 + 311.2*s^2 + 659.2*s + 388.3)

s=A2;

Gs_comp_A2=eval(Gs_comp2)

K2=1/abs(Gs_comp_A2)

% el controlador está dado por
RA2=K2*C2

figure(4)
hold on
rlocus(RA2*G)
plot(real(A2),imag(A2),'x')

% ahora dibujamos la respuesta al escalón

G_lc2=feedback(RA2*G,1)

figure(5)
hold on
step(G_lc2)
datos = stepinfo(G_lc2,'RiseTimeLimits',[0 1]);

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