%% ESERCIZIO
%CASO 1:  Bwi=20 [Hz]
%         m_phi= pi/3 [rad]  
%         Tau_M= 2e-3 [s]
          
%CASO 2:  Bwi=10 [Hz]
%         m_phi= pi/3 [rad]  
%         Tau_M= 1e-3 [s]
          
%CASO 3:  Bwi=10 [Hz]
%         m_phi= pi/4 [rad]  
%         Tau_M= 1e-3 [s]
%----------------------------------------------------------
Tc=10e-4;                       %Passo simulazione [s]



%ATTUATORE DI COPPIA
K_M=1;                          %Guadagno statico   [Nm/V]  
Tau_M=2e-3;                     %Costante di tempo  [s]  
AT=tf(K_M,[Tau_M 1]);           %FDT attuatore di coppia

%CARICO MECCANICO
J=0.01;                         %Momento d'inerzia [Kg*m^2]
LOAD=tf([1],[J 0]);             %FDT CARICO INERZIALE

%TRASDUTTORE DI VELOCITA'
K_Tw=10/(3000 *2*pi/60);        %Guadagno [V/(rad/s)]


FDT_sr= AT*LOAD*K_Tw;           %FDT ANELLO APERTO SENZA REGOLATORE
  
%% REGOLATORE
Bwi=20*2*pi;                    %Banda passante [rad/s]
m_phi=pi/3;                     %Margine di fase [rad]

Tau_R=1/Bwi*tan(m_phi+atan(Bwi*Tau_M));                             %Tau_R=Kp/Ki: Costante di tempo del regolatore [s]
Ki=Bwi^2*J*sqrt(1+(Bwi*Tau_M)^2)/(K_M*K_Tw*sqrt(1+(Bwi*Tau_R)^2));  %Guadagno Integrale
Kp=Tau_R*Ki;                                                        %Guadagno Proporzionale

PI=tf([Kp Ki],[1 0]);                     %FDT REGOLATORE PI

Out_lim=10;                               %Limitazione regolatore [Nm]
Int_lim=10;%inf;                          %Limitazione parte integrale [Nm]  : inf per disattivare anti-windup

%%
FDT= PI*FDT_sr;                             %FDT ANELLO APERTO CON REGOLATORE
FDT_c=FDT/(1+FDT);                          %FDT ANELLO CHIUSO CON REGOLATORE

[a,m_phi_vero,b,Bwi_vero]=margin(FDT);      %Valuto margine di fase e frequenza di attraversamento
m_phi_vero
Bwi_vero

%======================= Plot Bode manuale ================================
% creo modulo e fase della funzione di trasferimento FDT
w = logspace(-1,5);
[mag_sr,phase_sr] = bode(FDT_sr,w);
[mag,phase] = bode(FDT,w);
[mag_c,phase_c] = bode(FDT_c,w);

lim = size(mag);
mag_G_sr=zeros(1,lim);
phase_G_sr=zeros(1,lim);
mag_G=zeros(1,lim);
phase_G=zeros(1,lim);
mag_G_c=zeros(1,lim);
phase_G_c=zeros(1,lim);
for i=1:lim(3)
   mag_G(i) = mag(1,1,i);
   phase_G(i) = phase(1,1,i);
   mag_G_sr(i) = mag_sr(1,1,i);
   phase_G_sr(i) = phase_sr(1,1,i);
   mag_G_c(i) = mag_c(1,1,i);
   phase_G_c(i) = phase_c(1,1,i);
end


%
figure('name','FDT CATENA APERTA');
subplot(2,1,1);
title ('BODE FDT CATENA APERTA', 'fontsize', 18);
semilogx(w,20*log10(mag_G), 'LineWidth', 3);
hold on;
grid on;
semilogx(w,20*log10(mag_G_sr),'m--', 'LineWidth', 3);
semilogx(1/Tau_R,0,'gx','MarkerSize',4,'LineWidth',15);
semilogx(1/Tau_M,0,'rx','MarkerSize',4,'LineWidth',15);
semilogx(Bwi_vero,0,'ko','MarkerSize',4,'LineWidth',15)
xlabel('Frequency (rad/s)', 'fontsize', 18)
ylabel('Gain (dB)', 'fontsize', 18);
legend ('FDT','FDT-sr','1/ \tau_R','1/ \tau_M','\nu_A')
set (gca, 'fontsize', 16);

subplot(2,1,2);
semilogx(w,phase_G, 'LineWidth', 3);
hold on;
grid on;
semilogx(w,phase_G_sr,'m--', 'LineWidth', 3);
semilogx(1/Tau_R,0,'gx','MarkerSize',4,'LineWidth',15);
semilogx(1/Tau_M,0,'rx','MarkerSize',4,'LineWidth',15);
semilogx(Bwi_vero,0,'ko','MarkerSize',4,'LineWidth',15)
xlabel('Frequency (rad/s)', 'fontsize', 18);
ylabel('Phase (deg)', 'fontsize', 18);
set (gca, 'fontsize', 16);




%
figure('name','FDT CATENA CHIUSA');
subplot(2,1,1);
title ('BODE FDT CATENA APERTA', 'fontsize', 18);
semilogx(w,20*log10(mag_G_c), 'LineWidth', 3);
hold on;
grid on;
semilogx(Bwi_vero,0,'ko','MarkerSize',4,'LineWidth',15)
xlabel('Frequency (rad/s)', 'fontsize', 18)
ylabel('Gain (dB)', 'fontsize', 18);
legend ('FDT_c','\nu_A')
set (gca, 'fontsize', 16);

subplot(2,1,2);
semilogx(w,phase_G_c, 'LineWidth', 3);
hold on;
grid on;
semilogx(Bwi_vero,0,'ko','MarkerSize',4,'LineWidth',15)
xlabel('Frequency (rad/s)', 'fontsize', 18);
ylabel('Phase (deg)', 'fontsize', 18);
set (gca, 'fontsize', 16);