clear; close all;
format long

% Diseño de un PID para controlar la velocidad de un motor CC
s=tf('s');
paso=0.00001; tfinal=1.8;
t=0:paso:tfinal-paso;
r=t;

% Definición de las matrices de estado del sistema  
Ra=0.2; La=0.005; b=0.1; J=1.85; Kt=0.2; Kb=0.2;
Va=250;
Vref=10;

% Función de transferencia W(s)/Va(s) de 2do orden
Gv=(Kt/(J*La))/(s^2 + ((Ra/La)+(b/J))*s + (Ra*b/(J*La)) + Kt*Kb/(J*La));

K=Kt;
tm=J*Ra/(b*Ra + K^2); Km=K/(b*Ra + K^2);

taue=La/Ra; taum=J/b;
% Función de transferencia W(s)/Va(s) aproximada de 1er orden 
Gva=(Km/tm)/(s + (1/tm));

% Prueba a LA con tensión nominal
figure; step(Va*Gva);
legend('Respuesta Planta a Lazo Abierto')

Hs=10/250;
% Hs=1;

Glcsc=feedback(Va*Gva,Hs);
figure; step(Vref*Glcsc,t)
legend('Respuesta Planta a Lazo Cerrado SC')

% Especificaciones
ts=0.4; xita=0.8; 
wn=4.5/(xita*ts);
Glcd=wn^2/(s^2 + 2*xita*wn*s + wn^2);
figure; step(Glcd)
polos=pole(Glcd);
sigma=abs(real(polos(1)));
wd=imag(polos(1));

Mp1=100*exp(-pi*xita/(sqrt(1 - xita^2)));
Mp2=100*exp(-pi*sigma/wd);

Kp=10; Kd=((Va*Hs*Km*Kp + 1) - (2*xita*wn*tm))/(2*xita*wn*Va*Hs*Km);  
Ki=(wn^2*(tm + Va*Hs*Kd*Km))/(Va*Hs*Km);

Ti=Kp/Ki; Td=Kd/Kp;

Gc=(s*Kp + Ki + s^2*Kd)/s;
zpk(Gc)

Gla=Gc*Va*Gva;
Glc=feedback(Gla,Hs);
zpk(Glc)
[Wnlc,xitalc]=damp(Glc);

figure; step(Vref*Glc,Vref*Glcsc,t)
legend('Planta LC compensada con PID','Planta LC no compensada')

figure; step(Vref*Glc,Va*Glcd,t)
legend('Planta LC compensada con PID','Planta LC deseada')

figure; pzmap(Glc,Glcsc)
legend('polos y ceros LC con PID','polos y ceros LC sin PID')

close all

%% Reajuste compensador

% sisotool(Va*Gva,Gc,Hs,Vref)

%     0.86247 (s+14.66) (s+5.584)
% C1  ---------------------------
%                s

%     0.033543 (s+187.1) (s+0.6647)
% C2  ---------------------------
%                s

% Gcr=(0.033543*s^2 + 6.298*s + 4.172)/s;
Kp=6.298; Ki=4.172; Kd=0.033543;

% Kp=(14.66 + 5.584)*0.86247; Ki=14.66*5.584*0.86247; Kd=0.86247;
Gcr=Kp + s*Kd + Ki/s;

Glar=Gcr*Va*Gva;
Glcr=feedback(Glar,Hs);
zpk(Glcr)
[Wnrlc,xitarlc]=damp(Glcr);

figure; rlocus(Glar*Hs)

figure; step(Vref*Glcr,Vref*Glcsc,t)
legend('Planta LC compensada PID Reajustado','Planta LC no compensada')

figure; step(Vref*Glcr,Va*Glcd,t)
legend('Planta LC compensada','Planta LC deseada')

figure; pzmap(Glcr,Glcsc)
legend('polos y ceros LC con PID Reajustado','polos y ceros LC sin PID')