%% GOL speed controll
Idw_1 = 1/2 * mdw * rd^2;

Inertia_vect = [Idw_1 (Idw_1+2*10^(-3))]';


% Ki_i = 0.01;
% 
% Gain_vect = [Ki_i 0.05 0.1]';

figure

for jj=1:2

 Idw_1 = Inertia_vect(jj,1);                                                % Inerzia solo ruota+motore

%Ki_i = Gain_vect(jj,1);

k_thetad = rap_rid;                                                        % Guadagno statico per F/B

k_omega = 1/Kc;                                                            % Costante di velocità (inverso della costante elettrica) [(rad/s)/V]

tau_V = Kp_v/Ki_v;                                                         % Costante di tempo anello esterno controllo velocità [s] 

tau_i = Kp_i/Ki_i;                                                         % Costante di tempo anello interno controllo corrente [s]

tau_m = Idw_1*R/(Kc*Kc*rap_rid);                                           % Costante di tempo meccanica [s]

zeta_contr = Ki_i*Idw_1/(Kc*Kc*rap_rid);                                   % Smorzamento anello di controllo interno

Gol=@(s) ((k_thetad*k_omega*Ki_v*Ki_i)*(tau_V*tau_i*(s).^2+(tau_i+tau_V)*(s)+1))./((s.^2).*((tau_m*tauE)*s.^2+(tau_m+tau_i*zeta_contr)*s+ zeta_contr+1));

s = linspace(10^-10,10^6,10^3)';

s = 1i*s;

Gol_compl = Gol(s);

Phase = angle(Gol_compl);

Phase = Phase*(180/pi);

Mod_Gol = abs(Gol_compl);

GoldB = 20*log(Mod_Gol);


subplot(2,1,1)
semilogx(abs(s),GoldB,'linewidth',1.5)
grid on
subtitle('Gol')
xlabel('\omega_l_o_g')
ylabel('|Gol|_d_B')
hold on
%ylim([-50 +50])
%xlim([10^-10 10^5])
%ylim([-50 200])

subplot(2,1,2)
semilogx(abs(s),Phase,'linewidth',1.5)
grid on
subtitle('Fase')
xlabel('\omega_l_o_g')
ylabel('Fase [°]')
%xlim([10^-10 10^5])
hold on
end
legend('I=0.0055 [kg\bulletm^2]','I=0.0075 [kg\bulletm^2]')

%% Gcl Speed controll
Idw_1 = 1/2 * mdw * rd^2;

Inertia_vect = [Idw_1 (Idw_1+2*10^(-3)) ]';

% Ki_i = 0.01;
% 
% Gain_vect = [Ki_i 0.05 0.1]';

figure

for jj=1:2

Idw_1 = Inertia_vect(jj,1);                                                % Inerzia solo ruota+motore

% Ki_i = Gain_vect(jj,1);

k_thetad = rap_rid;                                                        % Guadagno statico per F/B

k_omega = 1/Kc;                                                            % Costante di velocità (inverso della costante elettrica) [(rad/s)/V]

tau_V = Kp_v/Ki_v;                                                         % Costante di tempo anello esterno controllo velocità [s] 

tau_i = Kp_i/Ki_i;                                                         % Costante di tempo anello interno controllo corrente [s]

tau_m = Idw_1*R/(Kc*Kc*rap_rid);                                           % Costante di tempo meccanica [s]

zeta_contr = Ki_i*Idw_1/(Kc*Kc*rap_rid);                                   % Smorzamento anello di controllo interno

Gcl=@(s) ((k_thetad*k_omega*Ki_v*Ki_i)*(tau_V*tau_i*(s).^2+(tau_i+tau_V)*(s)+1))./((s.^2).*((tau_m*tauE)*s.^2+(tau_m+tau_i*zeta_contr)*s+ zeta_contr+1)+(k_thetad*k_omega*Ki_v*Ki_i)*(tau_V*tau_i*(s).^2+(tau_i+tau_V)*(s)+1));
s = linspace(1,10^6,10^3)';
s = 1i*s;
Gcl_compl = Gcl(s);
Phase_cl = angle(Gcl_compl);
Phase_cl = Phase_cl*(180/pi);
Mod_Gcl = abs(Gcl_compl);
GcldB = 20*log(Mod_Gcl);

subplot(2,1,1)
semilogx(abs(s),GcldB,'linewidth',1.5)
grid on
subtitle('Gcl')
xlabel('\omega_l_o_g')
ylabel('|Gcl|_d_B')
xlim([10^0 10^5])
ylim([-40 60])
hold on
%ylim([-50 +50])

subplot(2,1,2)
semilogx(abs(s),Phase_cl,'linewidth',1.5)
grid on
subtitle('Fase')
xlabel('\omega_l_o_g')
ylabel('Fase [°]')
xlim([10^0 10^5])
hold on
end 
legend('I=0.0055 [kg\bulletm^2]','I=0.0075 [kg\bulletm^2]')

%% Gcompliance speed controll
Idw_1 = 1/2 * mdw * rd^2;

Inertia_vect = [Idw_1 (Idw_1+2*10^(-3)) ]';

% Ki_i = 0.01;
% 
% Gain_vect = [Ki_i 0.05 0.1]';

figure

for jj=1:2

Idw_1 = Inertia_vect(jj,1);                                                % Inerzia solo ruota+motore

% Ki_i = Gain_vect(jj,1);

k_thetad = rap_rid;                                                        % Guadagno statico per F/B

k_omega = 1/Kc;                                                            % Costante di velocità (inverso della costante elettrica) [(rad/s)/V]

tau_V = Kp_v/Ki_v;                                                         % Costante di tempo anello esterno controllo velocità [s] 

tau_i = Kp_i/Ki_i;                                                         % Costante di tempo anello interno controllo corrente [s]

tau_m = Idw_1*R/(Kc*Kc*rap_rid);                                           % Costante di tempo meccanica [s]

zeta_contr = Ki_i*Idw_1/(Kc*Kc*rap_rid);                                   % Smorzamento anello di controllo interno

Gcomp=@(s) (rd/Idw_1)*(tauE*tau_m*(s).^2+(tau_m+tau_i*zeta_contr).*(s)+zeta_contr)./(s.*((s.^2)*(tau_m*tauE)+(tau_m+tau_i*zeta_contr).*s+ zeta_contr+1+(k_thetad*k_omega*Ki_v*Ki_i)*(tau_V*tau_i*(s).^2+(tau_i+tau_V)*(s)+1)./((s).^2)));
s = linspace(10^0,10^6,10^3)';
s = 1i*s;
Gcomp_complx = Gcomp(s);
Phase_comp = atan2(imag(Gcomp_complx),real(Gcomp_complx));
Phase_comp = Phase_comp*(180/pi);
Mod_Gcomp = abs(Gcomp_complx);
GcompdB = 20*log(Mod_Gcomp);

subplot(2,1,1)
semilogx(abs(s),GcompdB,'linewidth',1.5)
grid on
subtitle('Gcomp')
xlabel('\omega_l_o_g')
ylabel('|Gcomp|_d_B')
hold on
xlim([10^0 10^5])
%ylim([-500 0])
%ylim([-50 +50])

subplot(2,1,2)
semilogx(abs(s),Phase_comp,'linewidth',1.5)
grid on
subtitle('Fase')
xlabel('\omega_l_o_g')
ylabel('Fase [°]')
xlim([10^0 10^5])
%ylim([-180 0])
hold on
end 
legend('I=0.0055 [kg\bulletm^2]','I=0.0075 [kg\bulletm^2]')


