test-bench-nano-hexapod/matlab/init_hac_iff.m

97 lines
2.3 KiB
Mathematica
Raw Normal View History

2021-07-01 17:45:57 +02:00
%%
clear;
2021-06-30 23:22:08 +02:00
%% Jacobian
load('mat/jacobian.mat', 'J');
2021-07-01 17:45:57 +02:00
Jinv = pinv(J);
save('sim_data/J.mat', 'J', 'Jinv');
%% Saved HAC
% load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
%% Developed HAC
s = zpk('s');
a = 5; % Amount of phase lead / width of the phase lead / high frequency gain
wc = 2*pi*110; % Frequency with the maximum phase lead [rad/s]
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
H_lpf = 1/(1 + s/2/pi/300);
gm = 0.02;
xi = 0.5;
wn = 2*pi*700;
H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
Khac_iff_struts = -2.2e4 * ... % Gain
H_lead * ... % Lead
H_lpf * ... % Lead
H_notch * ... % Notch
(2*pi*100/s) * ... % Integrator
eye(6); % 6x6 Diagonal
%% Save Controller
load('sim_data/data_sim.mat', 'Ts')
Khac_iff_struts = c2d(Khac_iff_struts, Ts, 'Tustin');
save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');
%% Loop Gain
load('mat/damped_plant_enc_plates.mat', 'f', 'G_enc_iff_opt')
L_frf = zeros(size(G_enc_iff_opt));
for i = 1:size(G_enc_iff_opt, 1)
L_frf(i, :, :) = squeeze(G_enc_iff_opt(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
end
colors = colororder;
freqs = 2*logspace(1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
% Diagonal Elements FRF
plot(f, abs(L_frf(:,1,1)), 'color', colors(1,:), ...
'DisplayName', 'Diagonal');
for i = 2:6
plot(f, abs(L_frf(:,i,i)), 'color', colors(1,:), ...
'HandleVisibility', 'off');
end
plot(f, abs(L_frf(:,1,2)), 'color', [colors(2,:), 0.2], ...
'DisplayName', 'Off-Diag');
for i = 1:5
for j = i+1:6
plot(f, abs(L_frf(:,i,j)), 'color', [colors(2,:), 0.2], ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]);
ylim([1e-3, 1e2]);
legend('location', 'northeast');
grid;
ax2 = nexttile;
hold on;
for i =1:6
plot(f, 180/pi*angle(L_frf(:,i,i)), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180]);
grid;
linkaxes([ax1,ax2],'x');
xlim([1, 2e3]);