clear
close all

s=tf('s'); 

% Planta
Gp=1/(s*(s+6)*(s+10));
syslcc_nc=feedback(Gp,1);
figure; step(syslcc_nc);

T=0.1;
z=tf('z',T);
Gpd=c2d(Gp,T,'zoh');

%      0.0001137*(z + 2.5545)*(z + 0.1760)
%      -----------------------------------
%       (z - 1)*(z - 0.5488)*(z - 0.3679)
     
syslc_nc=feedback(Gpd,1);
figure; step(syslc_nc);

% Especificaciones de proyecto
% El tiempo de asentamiento del sistema compensado debe ser menor o igual a:
ts=2;                   % ts=4/sigma

% El sobrepaso maximo deseado debe ser menor o igual a 5%
Mp=5/100;               % Mp=exp(-pi.sigma/wd)

% La constante de error para entrada en rampa debe ser mayor o igual a 20:
Kv=20;

sigma=4/ts;             % Parte real de los polos dominantes de lazo cerrado
wd=-pi*sigma/log(Mp);   % Parte imaginaria de los polos dominantes de lazo cerrado
         
s1=-sigma+1i*wd;         % Polos dominantes de lazo cerrado
s2=-sigma-1i*wd;         % complejos conjugados en el dominio continuo

z1=exp(s1*T);           % Polos dominantes de lazo cerrado
z2=exp(s2*T);           % complejos conjugados en el dominio discreto

% Polos y ceros de la planta discreta;
PP=pole(Gpd);
ZZ=zero(Gpd);
pp1=PP(1,1);            % polo planta
pp2=PP(2,1);            % polo planta
pp3=PP(3,1);            % polo planta
zz1=abs(ZZ(1,1));       % cero planta
zz2=abs(ZZ(2,1));       % cero planta

figure; pzmap(Gpd);
hold on; pzmap((z1),(z2))

% Proyecto Compensador PD discreto
% Condicion de angulo
% Vectores de los polos y ceros a z1
v1=(pp1-real(z1))+1i*imag(z1);      %fi1
v2=(real(z1)-pp2)+1i*imag(z1);      %fi2
v3=(real(z1)-pp3)+1i*imag(z1);      %fi3 este se cancela con el cero del compensador

% Vector del polo del compensador a z1
%v3=(real(z1)-b) + j*imag(z1);     %tita_b: polo del compensador

% Vectores de los ceros a z1
v4=(real(z1)+zz1)+1i*imag(z1);      %tita1
v5=(real(z1)+zz2)+1i*imag(z1);      %tita2

% Angulos de los polos y ceros a z1
tita1=angle(v4); tita1g=tita1*180/pi;
tita2=angle(v5); tita2g=tita2*180/pi;
fi1=pi-angle(v1); fi1g=fi1*180/pi;
fi2=angle(v2); fi2g=fi2*180/pi;
fi3=angle(v3); fi3g=fi3*180/pi;

% Obtención del parametro "b" a partir de la condición de angulo
% fi_b=pi-arctg[imag(z1)/(real(z1) - b)];
alfa=pi - tita1 - tita2 + (fi1 + fi2);

b=((real(z1)*tan(alfa))+imag(z1))/tan(alfa);
v6=(b-real(z1)) + 1i*imag(z1);
fi_b=pi-angle(v6); fi_bg=fi_b*180/pi;

% Condicion de modulo
VAbs=0.0001137*abs(v4)*abs(v5)/(abs(v1)*abs(v2)*abs(v6));
K=1/VAbs;

% Funcion transferencia PI discreto          
% Gpi(z)=K*(z+a)/(z-1)
a=pp3;
Gcdad=K*(z-a)/(z-b);      % compensador de adelanto
a2=0.88; b2=0.99;
%Gcdat=(z-a2)/(z-b2);      % compensador de atraso
% figure; bode(Gcd);
Gcdat=1;
% FT lazo abierto
Glad=Gpd*Gcdad*Gcdat;
% figure; pzmap(Glad);

syslc_c=feedback(Glad,1);
% figure; pzmap(syslc_c)

t=0:T:15;
figure; step(syslc_c,t);
hold on
step(syslc_nc,t);
legend('compensado','no compensado');
 
figure; nyquist(Gpd); 
title('sistema sin compensación')
figure; nyquist(Glad); 
title('sistema con compensación')

figure; margin(Gpd); grid;
legend('sistema sin compensación')
figure; margin(Glad); grid;
legend('sistema con compensación')

figure; bode(syslc_nc); hold on;
bode(syslc_c); grid; legend('no compensado','compensado');

rampa=t;
figure; yr=lsim(syslc_c,rampa,t); plot(t,rampa,t,yr); grid
legend('respuesta entrada en rampa')

error=(rampa-yr')*100; figure; plot(t,error,'k'); grid
legend('error porcentual entrada en rampa')

% input('pulse enter para terminar'); clc;