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

%% Ejemplo 3 - control P de un servomecanismo

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

G=tf(num,den)

figure(1)
rlocus(G)

% Determinar K tal que SO<10% y ts<5

% Paso 1: vamos a approximar la G por una equivalente de orden 2

num1=num/5;
den1=d1

G1= tf(num1,den1)

figure(2)
rlocus(G1)

% Del lugar de las raices se deduce que Re=-0.025

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

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

wn=0.025/d

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

figure(2)
hold on
plot(real(A),imag(A),'x')
ylim([-.05 .05])

% por el criterio swl módulo 1+K*G(A)=0 ==> K=|1/G(A)|

% Escribimos G(s) en simbolico

syms s

Gs= 0.15/(s^2 + 0.05*s)

s=A;

G_A=eval(Gs)

K=1/abs(G_A)

G1_lc=feedback(K*G1,1) % aplico el controlador al sistema approx
G_lc=feedback(K*G,1) % aplico el controlador al sistema original

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

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

%% Ejemplo 4 - control PD de un servomecanismo PD=K(s+c)

% Diseñar un PD tal que SO<10% y ts<5. Ya tenemos d del ejemplo anterior.
% wn lo calculamos usando la formula del tiempo de subida: ts= (pi-alpha)/(wn*sqrt(1-d^2))

d

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

% podemos calcular donde tiene que hubicarse el polo en lazo cerrado

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

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

% en la figura se aprecia como A no pertenece al l.r.
% tenemos que modificar el l.r. para que esto ocurra. para poderlo hacer
% agregamos un cero (sería el cero del PD)

% Para agregar el cero usamo 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 59)

arg_Ac = pi + atan(imag(A)/real(A)) + atan(imag(A)/(real(A)+5)) + atan(imag(A)/(real(A)+0.05))

% arg_Ac=atan(imag(A)/(real(A)+c)) ---> tan(arg_Ac)=imag(A)/(real(A)+c)

c=(imag(A)/tan(arg_Ac))-real(A) % el cero real está en -c

figure(1)
hold on
plot(-real(c),imag(c),'o')

% creamos la función compensada C*G con C=K*(s+c). Tomando K=1 dibujamos el
% lugar de las raices de la nuena función

C=tf([1 c],1)

G_comp=C*G

figure(4)
hold on
grid on
rlocus(G_comp)
plot(-real(c),imag(c),'o')
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= (0.75*s + 0.3408)/(s^3 + 5.05*s^2 + 0.25*s)

s=A;

Gs_comp_A=eval(Gs_comp)

K=1/abs(Gs_comp_A)

% ahora dibujamos la respuesta al escalón

G_compK=K*G_comp

G_compK_lc=feedback(G_compK,1)

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

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

% para encontrar el punto B: dibujamos la recta que pasa por A y por B
% (mismo d por misma SO)

m=-tan(alpha)

vx=[]; vy=[];

for j=-3:0.1:0
    vx=[vx,j];
    vy=[vy,m*j];
end

figure(4)
hold on
plot(vx,vy)

% de la grafica podemos ver que la parte real(B)=-2.265 y imag(B)=3.09
% B=-2.265+3.09*i

% tambien podemos calcular estos valores analiticamente usando la asintota

sigma= (-5-0.05-0+c)/2 

% entonces definimos real(B)=sigma=-d*wn, y teniendo en cuenta que d se mantiene
% igual (misma SO)

wn=-sigma/d

% la parte imaginaria de B será imag(B)= wn*sqrt(1-d^2)

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

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

syms s

Gs_comp= (0.75*s + 0.3408)/(s^3 + 5.05*s^2 + 0.25*s)

s=B;

Gs_compB=eval(Gs_comp)

K=1/abs(Gs_compB)

% ahora dibujamos la respuesta al escalón

G_compK=K*G_comp

G_compK_lc=feedback(G_compK,1)

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

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