clear
close all
s=tf('s');

% Definición de parametros
Ra=0.8; La=0.005; b=0.1; J=2.2; Kt=1; Kb=1;
tm=J*Ra/(b*Ra+Kt*Kb); Km=Kt/(b*Ra+Kt*Kb);

tm=0.85; Km=16;
Gp=Km/(s*(tm*s + 1));
Glcsc=feedback(Gp,1);
[num,den]=tfdata(Glcsc,'v');

figure; step(Glcsc)

spec=stepinfo(Glcsc);

% RiseTime: 0.2676
% SettlingTime: 6.6365
% SettlingMin: 0.5775
% SettlingMax: 1.6503
% Overshoot: 65.0287
% Undershoot: 0
% Peak: 1.6503
% PeakTime: 0.7241

% Especificaciones
Mp=10/100; tp=0.9;
xita=-log(Mp)/(sqrt(log(Mp)^2 + pi^2));
wn=sqrt(den(3));
wd=pi/tp;
wn=wd/sqrt(1 - xita^2);

Kh=(2*xita*wn*tm - 1)/Km;
ts=4/(xita*wn);

Glc=(Km/tm)/(s^2 + s*((Kh*Km + 1)/tm) + (Km/tm));
 
% Glc =
%  
%           18.82
%   ---------------------
%   s^2 + 5.117 s + 18.82

t=0:0.0001:10;
figure; step(Glcsc,Glc,t); grid;
legend('Motor con Lazo Posicion','Motor con Lazo de Velocidad y Posicion')
r=t; 
figure; lsim(Glc,r,t); grid;

% Diagrama de Nyquist
Gcd=(Km/tm)/(s^2 + s*((Km*Kh + 1)/tm));
figure; nyquist(Gcd)

figure; bode(Gp,Gcd); grid
legend('Motor con Lazo Posicion','Motor con Lazo de Velocidad y Posicion')

figure; bode(Glcsc,Glc)
legend('Motor con Lazo Posicion','Motor con Lazo de Velocidad y Posicion')

figure; pzmap(Glcsc,Glc)
legend('Motor con Lazo Posicion','Motor con Lazo de Velocidad y Posicion')

close all

% Desplazamiento angular
Glcpos=(Km/tm)/(s^2 + s/tm + Km/tm);
% figure; step(Glcpos)

% Con PD
% z=Kp/Kd;
Kp=10; zero=4.3;
Kd=Kp/zero;
Glcpd=(Kp + s*Kd)*(Km/tm)/(s^2 + s*((Kd*Km + 1)/tm) + Kp*Km/tm);
figure; step(Glcpos,Glcpd)
legend('Planta LC sin Compensar','Planta LC con PD')

figure; pzmap(Glcpos,Glcpd)
legend('Planta LC sin Compensar','Planta LC con PD')

% Kp=10; zero=-2.3;
% Kd=-Kp/zero;
% Glcpd=(Kp + s*Kd)*(Km/tm)/(s^2 + s*((Kd*Km + 1)/tm) + Kp*Km/tm);
% figure; step(Glcpos,Glcpd)
% legend('Planta LC con PD')
% 
% Kp=1; zero=-1.3;
% Kd=-Kp/zero;
% Glcpd=(Kp + s*Kd)*(Km/tm)/(s^2 + s*((Kd*Km + 1)/tm) + Kp*Km/tm);
% figure; step(Glcpos,Glcpd)
% legend('Planta LC con PD')