Work on tomography experiment with active damping
This commit is contained in:
parent
1889b1be5c
commit
3367cabc70
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
280
active_damping/matlab/dvf.m
Normal file
280
active_damping/matlab/dvf.m
Normal file
@ -0,0 +1,280 @@
|
|||||||
|
%% Clear Workspace and Close figures
|
||||||
|
clear; close all; clc;
|
||||||
|
|
||||||
|
%% Intialize Laplace variable
|
||||||
|
s = zpk('s');
|
||||||
|
|
||||||
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||||
|
|
||||||
|
load('./active_damping/mat/plants.mat', 'G_dvf');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
K_dvf = s*20000/(1 + s/2/pi/10000);
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(K_dvf*G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
K_dvf = -K_dvf*eye(6);
|
||||||
|
|
||||||
|
save('./active_damping/mat/K_dvf.mat', 'K_dvf');
|
||||||
|
|
||||||
|
initializeReferences();
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
initializeNanoHexapod('actuator', 'piezo');
|
||||||
|
initializeSample('mass', 50);
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_iff = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_dvf = K_dvf;
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
|
K_dvf = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
|
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'sim_nass_active_damping';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
|
||||||
|
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
|
||||||
|
|
||||||
|
save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
subplot(2, 1, 1);
|
||||||
|
title('$D_g$ to $D$');
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'southeast');
|
||||||
|
|
||||||
|
subplot(2, 1, 2);
|
||||||
|
title('$F_s$ to $D$');
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 2, 1);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
ax2 = subplot(2, 2, 2);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
ax3 = subplot(2, 2, 3);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
legend('location', 'northwest');
|
||||||
|
|
||||||
|
ax4 = subplot(2, 2, 4);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
legend('location', 'northwest');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||||
|
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
|
||||||
|
initializeNanoHexapod('actuator', 'piezo');
|
||||||
|
initializeSample('mass', 50);
|
||||||
|
|
||||||
|
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||||
|
|
||||||
|
initDisturbances();
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_ine = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
K_iff = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_dvf = K_dvf;
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
|
|
||||||
|
load('mat/conf_simscape.mat');
|
||||||
|
set_param(conf_simscape, 'StopTime', '3');
|
||||||
|
|
||||||
|
sim('sim_nass_active_damping');
|
||||||
|
|
||||||
|
t_dvf = t;
|
||||||
|
Ern_dvf = Ern;
|
||||||
|
save('./active_damping/mat/tomo_exp.mat', 'Ern_dvf', 't_dvf', '-append');
|
||||||
|
|
||||||
|
load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_dvf', 't', 't_dvf');
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||||
|
plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||||
|
plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(t_dvf, Ern_dvf(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - DVF')
|
||||||
|
plot(t_dvf, Ern_dvf(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - DVF')
|
||||||
|
plot(t_dvf, Ern_dvf(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - DVF')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||||
|
plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||||
|
plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(t_dvf, Ern_dvf(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - DVF')
|
||||||
|
plot(t_dvf, Ern_dvf(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - DVF')
|
||||||
|
plot(t_dvf, Ern_dvf(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - DVF')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
300
active_damping/matlab/iff.m
Normal file
300
active_damping/matlab/iff.m
Normal file
@ -0,0 +1,300 @@
|
|||||||
|
%% Clear Workspace and Close figures
|
||||||
|
clear; close all; clc;
|
||||||
|
|
||||||
|
%% Intialize Laplace variable
|
||||||
|
s = zpk('s');
|
||||||
|
|
||||||
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||||
|
|
||||||
|
load('./active_damping/mat/undamped_plants.mat', 'G_iff');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
K_iff = 1000/s;
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(K_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
K_iff = -K_iff*eye(6);
|
||||||
|
|
||||||
|
save('./active_damping/mat/K_iff.mat', 'K_iff');
|
||||||
|
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
|
||||||
|
initializeNanoHexapod('actuator', 'piezo');
|
||||||
|
initializeSample('mass', 50);
|
||||||
|
|
||||||
|
initializeReferences();
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_ine = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
K_iff = K_iff;
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_dvf = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
|
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'sim_nass_active_damping';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
|
||||||
|
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
|
||||||
|
|
||||||
|
G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
||||||
|
% G_rmc = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
||||||
|
|
||||||
|
save('./active_damping/mat/plants.mat', 'G_iff', '-append');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
subplot(2, 1, 1);
|
||||||
|
title('$D_g$ to $D$');
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
subplot(2, 1, 2);
|
||||||
|
title('$F_s$ to $D$');
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(minreal(prescale(G_iff.G_dist('Dz', 'Frzz'), {2*pi, 2*pi*1e3})), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dz', 'Ftyz')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dx', 'Ftyx')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 2, 1);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
ax2 = subplot(2, 2, 2);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
ax3 = subplot(2, 2, 3);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
legend('location', 'northwest');
|
||||||
|
|
||||||
|
ax4 = subplot(2, 2, 4);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
legend('location', 'northwest');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
for ix = 1:6
|
||||||
|
for iy = 1:6
|
||||||
|
subplot(6, 6, (ix-1)*6 + iy);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart(ix, iy), freqs, 'Hz'))), 'k-');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart(ix, iy), freqs, 'Hz'))), 'k--');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylim([1e-12, 1e-5]);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
|
||||||
|
initializeNanoHexapod('actuator', 'piezo');
|
||||||
|
initializeSample('mass', 50);
|
||||||
|
|
||||||
|
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||||
|
|
||||||
|
initDisturbances();
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_ine = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
K_iff = K_iff;
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_dvf = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
|
|
||||||
|
load('mat/conf_simscape.mat');
|
||||||
|
set_param(conf_simscape, 'StopTime', '3');
|
||||||
|
|
||||||
|
sim('sim_nass_active_damping');
|
||||||
|
|
||||||
|
t_iff = t;
|
||||||
|
Ern_iff = Ern;
|
||||||
|
save('./active_damping/mat/tomo_exp.mat', 'Ern_iff', 't_iff', '-append');
|
||||||
|
|
||||||
|
load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_iff', 't', 't_iff');
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||||
|
plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||||
|
plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(t_iff, Ern_iff(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - IFF')
|
||||||
|
plot(t_iff, Ern_iff(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - IFF')
|
||||||
|
plot(t_iff, Ern_iff(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - IFF')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||||
|
plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||||
|
plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(t_iff, Ern_iff(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - IFF')
|
||||||
|
plot(t_iff, Ern_iff(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - IFF')
|
||||||
|
plot(t_iff, Ern_iff(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - IFF')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
263
active_damping/matlab/ine.m
Normal file
263
active_damping/matlab/ine.m
Normal file
@ -0,0 +1,263 @@
|
|||||||
|
%% Clear Workspace and Close figures
|
||||||
|
clear; close all; clc;
|
||||||
|
|
||||||
|
%% Intialize Laplace variable
|
||||||
|
s = zpk('s');
|
||||||
|
|
||||||
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||||
|
|
||||||
|
load('./active_damping/mat/undamped_plants.mat', 'G_ine');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
K_ine = 1e3/(1+s/(2*pi*100));
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(K_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i=1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(K_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
K_ine = -K_ine*eye(6);
|
||||||
|
|
||||||
|
save('./active_damping/mat/K_ine.mat', 'K_ine');
|
||||||
|
|
||||||
|
initializeReferences();
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
initializeNanoHexapod('actuator', 'piezo');
|
||||||
|
initializeSample('mass', 50);
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_iff = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_ine = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
K_ine = -K_ine*eye(6);
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
|
||||||
|
G_ine = identifyPlant();
|
||||||
|
|
||||||
|
save('./active_damping/mat/plants.mat', 'G_ine', '-append');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
subplot(2, 1, 1);
|
||||||
|
title('$D_g$ to $D$');
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
subplot(2, 1, 2);
|
||||||
|
title('$F_s$ to $D$');
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 2, 1);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
ax2 = subplot(2, 2, 2);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
ax3 = subplot(2, 2, 3);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
legend('location', 'northwest');
|
||||||
|
|
||||||
|
ax4 = subplot(2, 2, 4);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
legend('location', 'northwest');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||||
|
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
|
||||||
|
initializeNanoHexapod('actuator', 'piezo');
|
||||||
|
initializeSample('mass', 50);
|
||||||
|
|
||||||
|
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||||
|
|
||||||
|
initDisturbances();
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_ine = K_ine;
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
K_iff = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_dvf = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
|
|
||||||
|
load('mat/conf_simscape.mat');
|
||||||
|
set_param(conf_simscape, 'StopTime', '3');
|
||||||
|
|
||||||
|
sim('sim_nass_active_damping');
|
||||||
|
|
||||||
|
t_ine = t;
|
||||||
|
Ern_ine = Ern;
|
||||||
|
save('./active_damping/mat/tomo_exp.mat', 'Ern_ine', 't_ine', '-append');
|
||||||
|
|
||||||
|
load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_ine', 't', 't_ine');
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||||
|
plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||||
|
plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(t_ine, Ern_ine(:,1), '--', 'DisplayName', '$\epsilon_{x}$ - Inertial')
|
||||||
|
plot(t_ine, Ern_ine(:,2), '--', 'DisplayName', '$\epsilon_{y}$ - Inertial')
|
||||||
|
plot(t_ine, Ern_ine(:,3), '--', 'DisplayName', '$\epsilon_{z}$ - Inertial')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||||
|
plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||||
|
plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(t_ine, Ern_ine(:,4), '--', 'DisplayName', '$\epsilon_{\theta_x}$ - Inertial')
|
||||||
|
plot(t_ine, Ern_ine(:,5), '--', 'DisplayName', '$\epsilon_{\theta_y}$ - Inertial')
|
||||||
|
plot(t_ine, Ern_ine(:,6), '--', 'DisplayName', '$\epsilon_{\theta_z}$ - Inertial')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
Binary file not shown.
166
active_damping/matlab/undamped_system.m
Normal file
166
active_damping/matlab/undamped_system.m
Normal file
@ -0,0 +1,166 @@
|
|||||||
|
%% Clear Workspace and Close figures
|
||||||
|
clear; close all; clc;
|
||||||
|
|
||||||
|
%% Intialize Laplace variable
|
||||||
|
s = zpk('s');
|
||||||
|
|
||||||
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||||
|
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
|
||||||
|
initializeNanoHexapod('actuator', 'piezo');
|
||||||
|
initializeSample('mass', 50);
|
||||||
|
|
||||||
|
initializeReferences();
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_ine = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
K_iff = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_dvf = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
|
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'sim_nass_active_damping';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
|
||||||
|
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
|
||||||
|
'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
|
||||||
|
|
||||||
|
G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
||||||
|
G_dvf = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
||||||
|
G_ine = minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
||||||
|
|
||||||
|
save('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [(m/s)/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
|
||||||
|
initDisturbances();
|
||||||
|
|
||||||
|
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||||
|
|
||||||
|
load('mat/conf_simscape.mat');
|
||||||
|
set_param(conf_simscape, 'StopTime', '3');
|
||||||
|
|
||||||
|
sim('sim_nass_active_damping');
|
||||||
|
|
||||||
|
save('./active_damping/mat/tomo_exp.mat', 'Ern', 't');
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||||
|
plot(t, Ern(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||||
|
plot(t, Ern(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
|
xlabel('Time [s]'); ylabel('Position Error [m]');
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(t, Ern(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||||
|
plot(t, Ern(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||||
|
plot(t, Ern(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||||
|
hold off;
|
||||||
|
xlim([1,inf]);
|
||||||
|
legend();
|
||||||
|
xlabel('Time [s]'); ylabel('Position Error [rad]');
|
||||||
30
active_damping/src/prepareTomographyExperiment.m
Normal file
30
active_damping/src/prepareTomographyExperiment.m
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
function [] = prepareTomographyExperiment(args)
|
||||||
|
|
||||||
|
arguments
|
||||||
|
args.nass_actuator char {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
|
||||||
|
args.sample_mass (1,1) double {mustBeNumeric, mustBePositive} = 50
|
||||||
|
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||||
|
end
|
||||||
|
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
|
||||||
|
initializeNanoHexapod('actuator', args.nass_actuator);
|
||||||
|
initializeSample('mass', args.sample_mass);
|
||||||
|
|
||||||
|
initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Ry_period);
|
||||||
|
|
||||||
|
K = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K', '-append');
|
||||||
|
K_ine = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||||
|
K_iff = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||||
|
K_dvf = tf(zeros(6));
|
||||||
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||||
BIN
figs/dvf_open_loop.png
Normal file
BIN
figs/dvf_open_loop.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 120 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 128 KiB After Width: | Height: | Size: 95 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 121 KiB After Width: | Height: | Size: 117 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 124 KiB After Width: | Height: | Size: 120 KiB |
BIN
figs/ine_open_loop_gain.png
Normal file
BIN
figs/ine_open_loop_gain.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 155 KiB |
BIN
figs/ine_plant.png
Normal file
BIN
figs/ine_plant.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 153 KiB |
BIN
figs/nass_act_damp_dvf_sim_tomo_rot.png
Normal file
BIN
figs/nass_act_damp_dvf_sim_tomo_rot.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 167 KiB |
BIN
figs/nass_act_damp_dvf_sim_tomo_trans.png
Normal file
BIN
figs/nass_act_damp_dvf_sim_tomo_trans.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 176 KiB |
BIN
figs/nass_act_damp_iff_sim_tomo_rot.png
Normal file
BIN
figs/nass_act_damp_iff_sim_tomo_rot.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 31 KiB |
BIN
figs/nass_act_damp_iff_sim_tomo_trans.png
Normal file
BIN
figs/nass_act_damp_iff_sim_tomo_trans.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 118 KiB |
BIN
figs/nass_act_damp_ine_sim_tomo_rot.png
Normal file
BIN
figs/nass_act_damp_ine_sim_tomo_rot.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 102 KiB |
BIN
figs/nass_act_damp_ine_sim_tomo_trans.png
Normal file
BIN
figs/nass_act_damp_ine_sim_tomo_trans.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 42 KiB |
BIN
figs/nass_act_damp_undamped_sim_tomo_rot.png
Normal file
BIN
figs/nass_act_damp_undamped_sim_tomo_rot.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 70 KiB |
BIN
figs/nass_act_damp_undamped_sim_tomo_trans.png
Normal file
BIN
figs/nass_act_damp_undamped_sim_tomo_trans.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
BIN
figs/nass_active_damping_iff_plant.png
Normal file
BIN
figs/nass_active_damping_iff_plant.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 120 KiB |
BIN
figs/nass_active_damping_ine_plant.png
Normal file
BIN
figs/nass_active_damping_ine_plant.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 95 KiB |
BIN
figs/nass_active_damping_inertial_plant.png
Normal file
BIN
figs/nass_active_damping_inertial_plant.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 154 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 122 KiB After Width: | Height: | Size: 120 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 115 KiB After Width: | Height: | Size: 95 KiB |
@ -107,7 +107,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
|||||||
|
|
||||||
%% Low Pass Filter to filter out the references
|
%% Low Pass Filter to filter out the references
|
||||||
s = zpk('s');
|
s = zpk('s');
|
||||||
w0 = 2*pi*100;
|
w0 = 2*pi*10;
|
||||||
xi = 1;
|
xi = 1;
|
||||||
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
@ -43,7 +43,7 @@ Tmax = args.Tmax;
|
|||||||
|
|
||||||
%% Low Pass Filter to filter out the references
|
%% Low Pass Filter to filter out the references
|
||||||
s = zpk('s');
|
s = zpk('s');
|
||||||
w0 = 2*pi*100;
|
w0 = 2*pi*10;
|
||||||
xi = 1;
|
xi = 1;
|
||||||
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user