%=================================================== 
%DISEÑO BASADO EN EL LUGAR GEOMETRICO DE LAS RAÍCES
%===================================================
%Ing. Jorge Duque
%CUTB Nov/1999 Cartagena
%Curso de Control Digital


%EJEMPLO 4.9 (OGATA, SISTEMAS DE CONTROL EN TIEMPO DISCRETO)

%Considere la siguiente planta:

%			           1
%         G(s) = -------
%                 s(s+2)
%Diseñe un controlador digital, en el plano z, de tal forma que los polos dominantes de lazo cerrado
%tengan:
%1) un factor de amortiguamiento relativo: zita = 0,5 
%2) Un tiempo de asentamiento: ts= 2seg

%El tiempo de muestreo es T= de 0.2 seg

%Obtenga la respuesta del sistema a una entrada escalón unitario y obtenga la constante de error de 
%velocidad Kv

%SOLUCION:

%1. UBICACION DESEADA DE LOS POLOS:


%ts =4/(zita*Wn) 
ts=2;
zita=0.5;
Wn=4/(zita*ts);
Wd=Wn*sqrt(1-zita^2);

%s1,2= -zita*Wn+-j*Wd

%2. SELECCION DEL PERIODO DE MUESTREO T:

% En este caso el período de muestreo está dado:

T=0.2

% La relacion Ws/Wd es el número de muestras por oscilación
% amortiguada y debe estar entre entre 8 y 10. 

%La frecuencia de muestreo Ws:

Ws=2*pi/T;

Ws/Wd

%ans = 9.0690; se comprueba que el período de muestreo es satisfactorio


%3. OBTENCION DE LA FUNCION DE TRANSFERENCIA DISCRETA:
%Con un período de muestreo de 0.2 seg, la función de transferencia discreta, precedida
%de un retenedor de orden cero,se obtiene como sigue:

num=1;
den=conv([1 0],[1 2]);
printsys(num,den,'s')

[numd,dend]=c2dm(num,den,T,'zoh');
printsys(numd,dend,'z')

%numd/dend = 
 
%     0.01758 z + 0.015388
%  ------------------------
%   z^2 - 1.6703 z + 0.67032

%4. LOCALIZACION DE LOS POLOS DE LAZO CERRADO EN EL PLANO Z:

% Los polos dominantes en lazo cerradoen el plano Z, se localizan utilizando
% IZI=e^(-zita*T*Wn) la cual se denominará GANANCIA(GAN)
% y el angulo ANG=2*pi(wd/ws) 

GAN=exp(-zita*T*Wn)
ANG=2*pi*(Wd/Ws)

% Obtenemos:

% GAN=0.6703   ANG =0.6928

polo=exp([-zita*Wn+j*Wn*sqrt(1-zita^2)]*T)

%polo =  0.5158 + 0.4281i.

figure(1)

pzmap(numd,dend);hold 

%límites
wd_i=[0:pi/(20*T):pi/T]'/(2*pi/T);
zita_i=[0:.05:1]';
ws_i=[0:pi/20:pi]';
zd=exp([-2*pi*zita/sqrt(1-zita^2)*wd_i+j*2*pi*wd_i]);
wd=exp([-zita_i*Wn+j*Wn*sqrt(1-zita_i.^2)]*(T));
plot(zd);
hold on
plot(conj(zd));
plot(wd)
plot(conj(wd))
%círculo unitario
plot(exp(j*[0:pi/20:pi]));plot(conj(exp(j*[0:pi/20:pi])))
hold off

%5. LUGAR DE LAS RAICES SIN COMPENSAR:


pause
figure(2)
rlocus(numd,dend);hold
%límites
wd_i=[0:pi/(20*T):pi/T]'/(2*pi/T);
zita_i=[0:.05:1]';
ws_i=[0:pi/20:pi]';
zd=exp([-2*pi*zita/sqrt(1-zita^2)*wd_i+j*2*pi*wd_i]);
wd=exp([-zita_i*Wn+j*Wn*sqrt(1-zita_i.^2)]*(T));
plot(zd);
hold on
plot(conj(zd));
plot(wd)
plot(conj(wd))
%círculo unitario
plot(exp(j*[0:pi/20:pi]));plot(conj(exp(j*[0:pi/20:pi])))
title('LUGAR DE RAICES PLANTA SIN COMPENSAR') 	
hold off

%6. CALCULO DE LA DEFICIENCIA ANGULAR
%La ubicación deseada del polo dominante es:
polo=exp([-zita*Wn+j*Wn*sqrt(1-zita^2)]*T)
%La suma de las contribuciones angulares en el polo dominante es:
[p]=roots(dend);
angulo=(angle(polyval(numd,polo))-angle(polo-p(1))-angle(polo-p(2)))/pi*180
%La deficiencia angular es:angulo+180
%El compensador debe proporcionar, éste ángulo:
%Se decide cancelar el polo en:p(2)
%Luego, el cero debe ser :-p(2)
ang=(angulo+180+angle(polo-p(2))/pi*180);
%Ahora se calcula la posición del polo
z=1/(tan(ang/180*pi))*imag(polo)-real(polo)
pause
%La función de transferencia del controlador:')
numgdd=[1 -p(2)];
dengdd=[1 z];

printsys(numgdd,dengdd,'z')
pause

%7.DETERMINACION DE LA GANANCIA:
%La ganancia K determinada en la gráfica:
figure(3)
rlocus(conv(numd,numgdd),conv(dend,dengdd))
title('LUGAR DE LAS RAICES DE LA PLANTA COMPENSADA') 
hold
%límites
wd_i=[0:pi/(20*T):pi/T]'/(2*pi/T);
zita_i=[0:.05:1]';
ws_i=[0:pi/20:pi]';
zd=exp([-2*pi*zita/sqrt(1-zita^2)*wd_i+j*2*pi*wd_i]);
wd=exp([-zita_i*Wn+j*Wn*sqrt(1-zita_i.^2)]*(T));
plot(zd);
hold on
plot(conj(zd));
plot(wd)
plot(conj(wd))
%círculo unitario
plot(exp(j*[0:pi/20:pi]));plot(conj(exp(j*[0:pi/20:pi])))

rlocfind(conv(numd,numgdd),conv(dend,dengdd))
hold off
pause

% ans = 12.4880

%la ganancia K determinada de la condición de magnitud es:')

K=polyval(conv(dend,dengdd),polo)/polyval(conv(numd,numgdd),polo);
K=abs(real(K))

numgdd1=K*numgdd;

pause
%El controlador diseñado es:')
printsys(numgdd1,dengdd,'z')
%Ahora se observa la respuesta a una entrada paso')
pause
[numds,dends]=series(numgdd1,dengdd,numd,dend)
[numdcp,dendcp]=cloop(numds,dends,-1)
figure(4)
y1=dstep(numdcp,dendcp,(5+T)./T);
	[m,p]=size(y1);
	x=linspace(0,5,m);
	stairs(x',y1)
   title('RESPUESTA PASO DE LA PLANTA COMPENSADA')
disp(' La constante Kv es:')

numk= conv([1 -1],numds)
denk= T.*conv([1 0],dends)
Kv= -(polyval(numk,1))/(polyval(denk,1))