New configurable simscape model. Add logging block
This commit is contained in:
parent
aa2f3254c2
commit
b54db6fa0d
2
.SimulinkProject/Root.type.Files/simscape.type.File.xml
Normal file
2
.SimulinkProject/Root.type.Files/simscape.type.File.xml
Normal file
@ -0,0 +1,2 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
@ -0,0 +1,2 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
@ -0,0 +1,6 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="artifact" />
|
||||
</Category>
|
||||
</Info>
|
||||
@ -0,0 +1,2 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
@ -0,0 +1,6 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
@ -0,0 +1,2 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Ref="simscape" Type="Relative" />
|
||||
@ -96,7 +96,7 @@ After that, a tomography experiment is simulation without any active damping tec
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Identification of the dynamics for Active Damping
|
||||
@ -113,11 +113,11 @@ We identify the dynamics of the system using the =linearize= function.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Relative Motion Outputs
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm'); io_i = io_i + 1; % Absolute Velocity Outputs
|
||||
@ -272,12 +272,12 @@ We identify the dynamics of the system using the =linearize= function.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1; % Metrology Outputs
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
@ -420,8 +420,8 @@ We initialize elements for the tomography experiment.
|
||||
|
||||
We change the simulation stop time.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '4.5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '4.5');
|
||||
#+end_src
|
||||
|
||||
And we simulate the system.
|
||||
@ -540,8 +540,8 @@ This is done in order for the transient phase to be over.
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
load('mat/conf_simscape.mat');
|
||||
open('nass_model.slx')
|
||||
load('mat/conf_simulink.mat');
|
||||
#+end_src
|
||||
|
||||
** Variation of the Sample Mass
|
||||
@ -561,7 +561,7 @@ We initialize all the stages with the default parameters.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -741,7 +741,7 @@ We initialize all the stages with the default parameters.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -920,7 +920,7 @@ We initialize all the stages with the default parameters.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -1252,7 +1252,7 @@ We initialize all the stages with the default parameters.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -1440,7 +1440,7 @@ We initialize all the stages with the default parameters.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -1710,7 +1710,7 @@ The control architecture is represented in figure [[fig:iff_1dof]] where one of
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Control Design
|
||||
@ -1832,7 +1832,7 @@ We initialize all the stages with the default parameters.
|
||||
We set the IFF controller.
|
||||
#+begin_src matlab
|
||||
load('./active_damping/mat/K_iff.mat', 'K_iff');
|
||||
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||
initializeController('type', 'iff', 'K', K_iff);
|
||||
#+end_src
|
||||
|
||||
We identify the dynamics of the system using the =linearize= function.
|
||||
@ -1842,7 +1842,7 @@ We identify the dynamics of the system using the =linearize= function.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -2023,13 +2023,13 @@ We initialize elements for the tomography experiment.
|
||||
We set the IFF controller.
|
||||
#+begin_src matlab
|
||||
load('./active_damping/mat/K_iff.mat', 'K_iff');
|
||||
save('./mat/controllers.mat', 'K_iff', '-append');
|
||||
initializeController('type', 'iff', 'K', K_iff);
|
||||
#+end_src
|
||||
|
||||
We change the simulation stop time.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '4.5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '4.5');
|
||||
#+end_src
|
||||
|
||||
And we simulate the system.
|
||||
@ -2191,7 +2191,7 @@ The actuator displacement can be measured with a capacitive sensor for instance.
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Control Design
|
||||
@ -2312,7 +2312,7 @@ We initialize all the stages with the default parameters.
|
||||
We set the DVF controller.
|
||||
#+begin_src matlab
|
||||
load('./active_damping/mat/K_dvf.mat', 'K_dvf');
|
||||
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||
initializeController('type', 'dvf', 'K', K_dvf);
|
||||
#+end_src
|
||||
|
||||
We identify the dynamics of the system using the =linearize= function.
|
||||
@ -2322,7 +2322,7 @@ We identify the dynamics of the system using the =linearize= function.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -2503,13 +2503,13 @@ We initialize elements for the tomography experiment.
|
||||
We set the DVF controller.
|
||||
#+begin_src matlab
|
||||
load('./active_damping/mat/K_dvf.mat', 'K_dvf');
|
||||
save('./mat/controllers.mat', 'K_dvf', '-append');
|
||||
initializeController('type', 'dvf', 'K', K_dvf);
|
||||
#+end_src
|
||||
|
||||
We change the simulation stop time.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '4.5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '4.5');
|
||||
#+end_src
|
||||
|
||||
And we simulate the system.
|
||||
@ -2668,7 +2668,7 @@ In Inertial Control, a feedback is applied between the measured *absolute* motio
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Control Design
|
||||
@ -2787,7 +2787,7 @@ We initialize all the stages with the default parameters.
|
||||
We set the Inertial controller.
|
||||
#+begin_src matlab
|
||||
load('./active_damping/mat/K_ine.mat', 'K_ine');
|
||||
save('./mat/controllers.mat', 'K_ine', '-append');
|
||||
initializeController('type', 'ine', 'K', K_ine);
|
||||
#+end_src
|
||||
|
||||
We identify the dynamics of the system using the =linearize= function.
|
||||
@ -2797,7 +2797,7 @@ We identify the dynamics of the system using the =linearize= function.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -3438,16 +3438,19 @@ We set the references and disturbances to zero.
|
||||
initializeDisturbances('enable', false);
|
||||
#+end_src
|
||||
|
||||
And all the controllers are set to 0.
|
||||
We set the controller type to Open-Loop.
|
||||
#+begin_src matlab
|
||||
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');
|
||||
initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
And we put some gravity.
|
||||
#+begin_src matlab
|
||||
initializeSimscapeConfiguration('gravity', true);
|
||||
#+end_src
|
||||
|
||||
We do not need to log any signal.
|
||||
#+begin_src matlab
|
||||
initializeLoggingConfiguration('log', 'none');
|
||||
#+end_src
|
||||
|
||||
** prepareTomographyExperiment
|
||||
@ -3511,16 +3514,19 @@ We set the references that corresponds to a tomography experiment.
|
||||
initializeDisturbances();
|
||||
#+end_src
|
||||
|
||||
And all the controllers are set to 0.
|
||||
Open Loop.
|
||||
#+begin_src matlab
|
||||
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');
|
||||
initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
And we put some gravity.
|
||||
#+begin_src matlab
|
||||
initializeSimscapeConfiguration('gravity', true);
|
||||
#+end_src
|
||||
|
||||
We log the signals.
|
||||
#+begin_src matlab
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
#+end_src
|
||||
|
||||
* TODO Order :noexport:
|
||||
@ -3532,7 +3538,7 @@ And all the controllers are set to 0.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -3554,7 +3560,7 @@ And all the controllers are set to 0.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -3638,7 +3644,7 @@ And all the controllers are set to 0.
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nass_active_damping';
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
@ -3682,103 +3688,6 @@ Why don't we see any resonance?
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
*** TODO test on hexapod
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'test_nano_hexapod';
|
||||
|
||||
%% 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, '/x'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'x', 'y', 'z'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'test_nano_hexapod';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Fx'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'Fx'};
|
||||
G.OutputName = {'x', 'y', 'z'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'southwest')
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
|
||||
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');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'southwest')
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
|
||||
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');
|
||||
#+end_src
|
||||
|
||||
*** Sensitivity to disturbances
|
||||
The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undamped]].
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -4,9 +4,12 @@ clear; close all; clc;
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('active_damping/src/');
|
||||
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
|
||||
load('./active_damping/mat/plants.mat', 'G_dvf');
|
||||
load('./active_damping/mat/undamped_plants.mat', 'G_dvf');
|
||||
load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_dvf');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
@ -14,8 +17,8 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, abs(squeeze(freqresp(-Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
@ -23,18 +26,20 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(-Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
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]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
K_dvf = s*20000/(1 + s/2/pi/10000);
|
||||
K_dvf = s*30000/(1 + s/2/pi/10000);
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
@ -42,23 +47,25 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, abs(squeeze(freqresp(K_dvf*Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylabel('Loop Gain'); 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
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]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
@ -66,26 +73,10 @@ 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);
|
||||
prepareLinearizeIdentification();
|
||||
|
||||
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');
|
||||
load('./active_damping/mat/K_dvf.mat', 'K_dvf');
|
||||
initializeController('type', 'dvf', 'K', K_dvf);
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
@ -96,185 +87,193 @@ 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, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); 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'};
|
||||
load('./active_damping/mat/cart_plants.mat', 'masses');
|
||||
|
||||
save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
|
||||
G_cart_dvf = {zeros(length(masses))};
|
||||
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
for i = 1:length(masses)
|
||||
initializeSample('mass', masses(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, 0.3, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
|
||||
|
||||
G_cart = G*inv(nano_hexapod.J');
|
||||
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
|
||||
G_cart_dvf(i) = {G_cart};
|
||||
end
|
||||
|
||||
save('./active_damping/mat/cart_plants.mat', 'G_cart_dvf', '-append');
|
||||
|
||||
load('./active_damping/mat/cart_plants.mat', 'masses', 'G_cart', 'G_cart_dvf');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
subplot(2, 1, 1);
|
||||
title('$D_g$ to $D$');
|
||||
ax1 = subplot(2, 1, 1);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_dvf{i}('Dnx', 'Fnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_dvf{i}('Dny', 'Fny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_dvf{i}('Dnz', 'Fnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend('location', 'northeast');
|
||||
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
ax2 = subplot(2, 1, 2);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_dvf{i}('Dnx', 'Fnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_dvf{i}('Dny', 'Fny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_dvf{i}('Dnz', 'Fnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
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]);
|
||||
legend('location', 'northwest');
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_dvf{i}('Rnx', 'Mnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_dvf{i}('Rny', 'Mny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_dvf{i}('Rnz', 'Mnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_dvf{i}('Rnx', 'Mnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_dvf{i}('Rny', 'Mny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_dvf{i}('Rnz', 'Mnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
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]);
|
||||
legend('location', 'northwest');
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 50);
|
||||
figure;
|
||||
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||
for ix = 1:6
|
||||
for iy = 1:6
|
||||
subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart{1}(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart_dvf{1}(ix, iy), freqs, 'Hz'))), 'k--');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-13, 1e-4]);
|
||||
xticks([])
|
||||
yticks([])
|
||||
end
|
||||
end
|
||||
|
||||
initDisturbances();
|
||||
prepareTomographyExperiment();
|
||||
|
||||
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('./active_damping/mat/K_dvf.mat', 'K_dvf');
|
||||
initializeController('type', 'dvf', 'K', K_dvf);
|
||||
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '3');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '4.5');
|
||||
|
||||
sim('sim_nass_active_damping');
|
||||
|
||||
t_dvf = t;
|
||||
Ern_dvf = Ern;
|
||||
save('./active_damping/mat/tomo_exp.mat', 'Ern_dvf', 't_dvf', '-append');
|
||||
En_dvf = En;
|
||||
Eg_dvf = Eg;
|
||||
save('./active_damping/mat/tomo_exp.mat', 'En_dvf', 'Eg_dvf', '-append');
|
||||
|
||||
load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_dvf', 't', 't_dvf');
|
||||
load('./active_damping/mat/tomo_exp.mat', 'En', 'En_dvf');
|
||||
Fs = 1e3; % Sampling Frequency of the Data
|
||||
t = (1/Fs)*[0:length(En(:,1))-1];
|
||||
|
||||
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]);
|
||||
plot(En(:,1), En(:,2), 'DisplayName', '$\epsilon_{x,y}$ - OL')
|
||||
plot(En_dvf(:,1), En_dvf(:,2), 'DisplayName', '$\epsilon_{x,y}$ - DVF')
|
||||
xlabel('X Motion [m]'); ylabel('Y Motion [m]');
|
||||
legend();
|
||||
|
||||
figure;
|
||||
ax1 = subplot(3, 1, 1);
|
||||
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]);
|
||||
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||
plot(t, En_dvf(:,1), 'DisplayName', '$\epsilon_{x}$ - DVF')
|
||||
legend();
|
||||
|
||||
ax2 = subplot(3, 1, 2);
|
||||
hold on;
|
||||
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||
plot(t, En_dvf(:,2), 'DisplayName', '$\epsilon_{y}$ - DVF')
|
||||
legend();
|
||||
ylabel('Position Error [m]');
|
||||
|
||||
ax3 = subplot(3, 1, 3);
|
||||
hold on;
|
||||
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||
plot(t, En_dvf(:,3), 'DisplayName', '$\epsilon_{z}$ - DVF')
|
||||
legend();
|
||||
xlabel('Time [s]');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([0.5,inf]);
|
||||
|
||||
figure;
|
||||
ax1 = subplot(3, 1, 1);
|
||||
hold on;
|
||||
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||
plot(t, En_dvf(:,4), 'DisplayName', '$\epsilon_{\theta_x}$ - DVF')
|
||||
legend();
|
||||
|
||||
ax2 = subplot(3, 1, 2);
|
||||
hold on;
|
||||
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||
plot(t, En_dvf(:,5), 'DisplayName', '$\epsilon_{\theta_y}$ - DVF')
|
||||
legend();
|
||||
ylabel('Position Error [rad]');
|
||||
|
||||
ax3 = subplot(3, 1, 3);
|
||||
hold on;
|
||||
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||
plot(t, En_dvf(:,6), 'DisplayName', '$\epsilon_{\theta_z}$ - DVF')
|
||||
legend();
|
||||
xlabel('Time [s]');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([0.5,inf]);
|
||||
|
||||
@ -4,18 +4,21 @@ clear; close all; clc;
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('active_damping/src/');
|
||||
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
|
||||
load('./active_damping/mat/undamped_plants.mat', 'G_iff');
|
||||
load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_iff');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
freqs = logspace(-2, 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, abs(squeeze(freqresp(-Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
@ -23,18 +26,21 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(-Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
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]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
K_iff = 1000/s;
|
||||
w0 = 2*pi*50;
|
||||
K_iff = -5000/s * (s/w0)/(1 + s/w0);
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
@ -42,8 +48,8 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, abs(squeeze(freqresp(K_iff*Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
@ -51,14 +57,16 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
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]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
@ -66,28 +74,10 @@ K_iff = -K_iff*eye(6);
|
||||
|
||||
save('./active_damping/mat/K_iff.mat', 'K_iff');
|
||||
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
prepareLinearizeIdentification();
|
||||
|
||||
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');
|
||||
load('./active_damping/mat/K_iff.mat', 'K_iff');
|
||||
initializeController('type', 'iff', 'K', K_iff);
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
@ -98,131 +88,108 @@ 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, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); 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'};
|
||||
load('./active_damping/mat/cart_plants.mat', 'masses');
|
||||
|
||||
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'}));
|
||||
G_cart_iff = {zeros(length(masses))};
|
||||
|
||||
save('./active_damping/mat/plants.mat', 'G_iff', '-append');
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
for i = 1:length(masses)
|
||||
initializeSample('mass', masses(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, 0.3, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
|
||||
|
||||
G_cart = G*inv(nano_hexapod.J');
|
||||
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
|
||||
G_cart_iff(i) = {G_cart};
|
||||
end
|
||||
|
||||
save('./active_damping/mat/cart_plants.mat', 'G_cart_iff', '-append');
|
||||
|
||||
load('./active_damping/mat/cart_plants.mat', 'masses', 'G_cart', 'G_cart_iff');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
subplot(2, 1, 1);
|
||||
title('$D_g$ to $D$');
|
||||
ax1 = subplot(2, 1, 1);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Dnx', 'Fnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Dny', 'Fny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Dnz', 'Fnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend('location', 'northeast');
|
||||
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
ax2 = subplot(2, 1, 2);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Dnx', 'Fnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Dny', 'Fny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Dnz', 'Fnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
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]);
|
||||
legend('location', 'northwest');
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Rnx', 'Mnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Rny', 'Mny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_iff{i}('Rnz', 'Mnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Rnx', 'Mnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Rny', 'Mny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_iff{i}('Rnz', 'Mnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
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]);
|
||||
legend('location', 'northwest');
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
@ -230,71 +197,84 @@ 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--');
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart{1}(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart_iff{1}(ix, iy), freqs, 'Hz'))), 'k--');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-12, 1e-5]);
|
||||
ylim([1e-13, 1e-4]);
|
||||
xticks([])
|
||||
yticks([])
|
||||
end
|
||||
end
|
||||
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
prepareTomographyExperiment();
|
||||
|
||||
initializeNanoHexapod('actuator', 'piezo');
|
||||
initializeSample('mass', 50);
|
||||
load('./active_damping/mat/K_iff.mat', 'K_iff');
|
||||
initializeController('type', 'iff', 'K', K_iff);
|
||||
|
||||
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');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '4.5');
|
||||
|
||||
sim('sim_nass_active_damping');
|
||||
|
||||
t_iff = t;
|
||||
Ern_iff = Ern;
|
||||
save('./active_damping/mat/tomo_exp.mat', 'Ern_iff', 't_iff', '-append');
|
||||
En_iff = En;
|
||||
Eg_iff = Eg;
|
||||
save('./active_damping/mat/tomo_exp.mat', 'En_iff', 'Eg_iff', '-append');
|
||||
|
||||
load('./active_damping/mat/tomo_exp.mat', 'Ern', 'Ern_iff', 't', 't_iff');
|
||||
load('./active_damping/mat/tomo_exp.mat', 'En', 'En_iff');
|
||||
Fs = 1e3; % Sampling Frequency of the Data
|
||||
t = (1/Fs)*[0:length(En(:,1))-1];
|
||||
|
||||
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();
|
||||
plot(En(:,1), En(:,2), 'DisplayName', '$\epsilon_{x,y}$ - OL')
|
||||
plot(En_iff(:,1), En_iff(:,2), 'DisplayName', '$\epsilon_{x,y}$ - IFF')
|
||||
xlabel('X Motion [m]'); ylabel('Y Motion [m]');
|
||||
legend('location', 'northwest');
|
||||
|
||||
figure;
|
||||
ax1 = subplot(3, 1, 1);
|
||||
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]);
|
||||
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||
plot(t, En_iff(:,1), 'DisplayName', '$\epsilon_{x}$ - IFF')
|
||||
legend('location', 'southwest');
|
||||
|
||||
ax2 = subplot(3, 1, 2);
|
||||
hold on;
|
||||
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||
plot(t, En_iff(:,2), 'DisplayName', '$\epsilon_{y}$ - IFF')
|
||||
legend('location', 'southwest');
|
||||
ylabel('Position Error [m]');
|
||||
|
||||
ax3 = subplot(3, 1, 3);
|
||||
hold on;
|
||||
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||
plot(t, En_iff(:,3), 'DisplayName', '$\epsilon_{z}$ - IFF')
|
||||
legend('location', 'northwest');
|
||||
xlabel('Time [s]');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([0.5,inf]);
|
||||
|
||||
figure;
|
||||
ax1 = subplot(3, 1, 1);
|
||||
hold on;
|
||||
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||
plot(t, En_iff(:,4), 'DisplayName', '$\epsilon_{\theta_x}$ - IFF')
|
||||
legend('location', 'northwest');
|
||||
|
||||
ax2 = subplot(3, 1, 2);
|
||||
hold on;
|
||||
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||
plot(t, En_iff(:,5), 'DisplayName', '$\epsilon_{\theta_y}$ - IFF')
|
||||
legend('location', 'southwest');
|
||||
ylabel('Position Error [rad]');
|
||||
|
||||
ax3 = subplot(3, 1, 3);
|
||||
hold on;
|
||||
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||
plot(t, En_iff(:,6), 'DisplayName', '$\epsilon_{\theta_z}$ - IFF')
|
||||
legend();
|
||||
xlabel('Time [s]');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([0.5,inf]);
|
||||
|
||||
@ -4,9 +4,12 @@ clear; close all; clc;
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('active_damping/src/');
|
||||
|
||||
open('active_damping/matlab/sim_nass_active_damping.slx')
|
||||
|
||||
load('./active_damping/mat/undamped_plants.mat', 'G_ine');
|
||||
load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_ine');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
@ -14,27 +17,29 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
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_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
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]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
K_ine = 1e3/(1+s/(2*pi*100));
|
||||
K_ine = 2.5e4;
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
@ -42,23 +47,25 @@ 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, abs(squeeze(freqresp(K_ine*Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylabel('Loop Gain'); 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'))));
|
||||
for i=1:length(masses)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(K_ine*Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
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]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
@ -66,198 +73,134 @@ 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);
|
||||
prepareLinearizeIdentification();
|
||||
|
||||
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');
|
||||
load('./active_damping/mat/K_ine.mat', 'K_ine');
|
||||
initializeController('type', 'ine', 'K', K_ine);
|
||||
|
||||
G_ine = identifyPlant();
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
save('./active_damping/mat/plants.mat', 'G_ine', '-append');
|
||||
%% 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, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
load('./active_damping/mat/cart_plants.mat', 'masses');
|
||||
|
||||
G_cart_ine = {zeros(length(masses))};
|
||||
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
for i = 1:length(masses)
|
||||
initializeSample('mass', masses(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, 0.3, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
|
||||
|
||||
G_cart = G*inv(nano_hexapod.J');
|
||||
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
|
||||
G_cart_ine(i) = {G_cart};
|
||||
end
|
||||
|
||||
save('./active_damping/mat/cart_plants.mat', 'G_cart_dvf', '-append');
|
||||
|
||||
load('./active_damping/mat/cart_plants.mat', 'masses', 'G_cart', 'G_cart_ine');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
subplot(2, 1, 1);
|
||||
title('$D_g$ to $D$');
|
||||
ax1 = subplot(2, 1, 1);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_ine{i}('Dnx', 'Fnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_ine{i}('Dny', 'Fny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_ine{i}('Dnz', 'Fnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend('location', 'northeast');
|
||||
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
ax2 = subplot(2, 1, 2);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_ine{i}('Dnx', 'Fnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_ine{i}('Dny', 'Fny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_ine{i}('Dnz', 'Fnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
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]);
|
||||
legend('location', 'northwest');
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
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');
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart_ine{i}('Rnx', 'Mnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart_ine{i}('Rny', 'Mny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart_ine{i}('Rnz', 'Mnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_ine{i}('Rnx', 'Mnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_ine{i}('Rny', 'Mny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart_ine{i}('Rnz', 'Mnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
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]);
|
||||
legend('location', 'northwest');
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
linkaxes([ax1,ax2],'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');
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
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();
|
||||
for ix = 1:6
|
||||
for iy = 1:6
|
||||
subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart{1}(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
plot(freqs, abs(squeeze(freqresp(G_cart_ine{1}(ix, iy), freqs, 'Hz'))), 'k--');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-13, 1e-4]);
|
||||
xticks([])
|
||||
yticks([])
|
||||
end
|
||||
end
|
||||
|
||||
@ -4,30 +4,11 @@ clear; close all; clc;
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('active_damping/src/');
|
||||
|
||||
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');
|
||||
prepareLinearizeIdentification();
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
@ -38,13 +19,13 @@ 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;
|
||||
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Relative Motion Outputs
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm'); io_i = io_i + 1; % Absolute Velocity Outputs
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G = linearize(mdl, io, 0.5, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
|
||||
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
|
||||
@ -56,6 +37,8 @@ G_ine = minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1
|
||||
|
||||
save('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');
|
||||
|
||||
load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
@ -63,7 +46,33 @@ 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'))));
|
||||
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');
|
||||
|
||||
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');
|
||||
@ -72,7 +81,7 @@ 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'))));
|
||||
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');
|
||||
@ -89,16 +98,16 @@ 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'))));
|
||||
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',[]);
|
||||
ylabel('Amplitude [$\frac{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(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
||||
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');
|
||||
@ -108,59 +117,169 @@ yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
prepareLinearizeIdentification();
|
||||
|
||||
%% 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; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1; % Metrology Outputs
|
||||
|
||||
masses = [1, 10, 50]; % [kg]
|
||||
|
||||
G_cart = {zeros(length(masses))};
|
||||
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
for i = 1:length(masses)
|
||||
initializeSample('mass', masses(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, 0.3, options);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
|
||||
|
||||
G_cart_i = G*inv(nano_hexapod.J');
|
||||
G_cart_i.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
|
||||
G_cart(i) = {G_cart_i};
|
||||
end
|
||||
|
||||
save('./active_damping/mat/cart_plants.mat', 'G_cart', 'masses');
|
||||
|
||||
load('./active_damping/mat/cart_plants.mat', 'G_cart', 'masses');
|
||||
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Dnx', 'Fnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Dny', 'Fny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Dnz', 'Fnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend([p1,p2,p3], {'Fx/Dx', 'Fy/Dx', 'Fz/Dz'});
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Dnx', 'Fnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Dny', 'Fny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Dnz', 'Fnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
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'))));
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p1 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Rnx', 'Mnx'), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p2 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Rny', 'Mny'), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
p3 = plot(freqs, abs(squeeze(freqresp(G_cart{i}('Rnz', 'Mnz'), freqs, 'Hz'))), ':');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [(m/s)/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
legend([p1,p2,p3], {'Rx/Mx', 'Ry/Mx', 'Rz/Mz'});
|
||||
|
||||
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'))));
|
||||
for i = 1:length(masses)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Rnx', 'Mnx'), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Rny', 'Mny'), freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_cart{i}('Rnz', 'Mnz'), freqs, 'Hz')))), ':', 'HandleVisibility', 'off');
|
||||
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]);
|
||||
yticks([-540:180:540]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
initDisturbances();
|
||||
prepareTomographyExperiment();
|
||||
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '3');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '4.5');
|
||||
|
||||
sim('sim_nass_active_damping');
|
||||
|
||||
save('./active_damping/mat/tomo_exp.mat', 'Ern', 't');
|
||||
save('./active_damping/mat/tomo_exp.mat', 'En', 'Eg', '-append');
|
||||
|
||||
load('./active_damping/mat/tomo_exp.mat', 'En');
|
||||
Fs = 1e3; % Sampling Frequency of the Data
|
||||
t = (1/Fs)*[0:length(En(:,1))-1];
|
||||
|
||||
figure;
|
||||
ax1 = subplot(3, 1, 1);
|
||||
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]');
|
||||
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
||||
legend('location', 'southwest');
|
||||
|
||||
ax2 = subplot(3, 1, 2);
|
||||
hold on;
|
||||
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
||||
legend('location', 'southwest');
|
||||
ylabel('Position Error [m]');
|
||||
|
||||
ax3 = subplot(3, 1, 3);
|
||||
hold on;
|
||||
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
||||
legend('location', 'northwest');
|
||||
xlabel('Time [s]');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([0.5,inf]);
|
||||
|
||||
figure;
|
||||
ax1 = subplot(3, 1, 1);
|
||||
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]);
|
||||
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
||||
legend('location', 'northwest');
|
||||
|
||||
ax2 = subplot(3, 1, 2);
|
||||
hold on;
|
||||
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
||||
legend('location', 'southwest');
|
||||
ylabel('Position Error [rad]');
|
||||
|
||||
ax3 = subplot(3, 1, 3);
|
||||
hold on;
|
||||
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
||||
legend();
|
||||
xlabel('Time [s]'); ylabel('Position Error [rad]');
|
||||
xlabel('Time [s]');
|
||||
|
||||
linkaxes([ax1,ax2,ax3],'x');
|
||||
xlim([0.5,inf]);
|
||||
|
||||
@ -20,11 +20,8 @@ initializeSample('mass', args.sample_mass);
|
||||
initializeReferences();
|
||||
initializeDisturbances('enable', false);
|
||||
|
||||
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');
|
||||
initializeController('type', 'open-loop');
|
||||
|
||||
initializeSimscapeConfiguration('gravity', true);
|
||||
|
||||
initializeLoggingConfiguration('log', 'none');
|
||||
|
||||
@ -22,11 +22,8 @@ initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Rz_period);
|
||||
|
||||
initializeDisturbances();
|
||||
|
||||
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');
|
||||
initializeController('type', 'open-loop');
|
||||
|
||||
initializeSimscapeConfiguration('gravity', true);
|
||||
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
|
||||
@ -203,8 +203,8 @@ Also, we measure the absolute displacement of the granite and of the top platfor
|
||||
|
||||
We load the configuration and we set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
|
||||
@ -79,8 +79,8 @@ The simulink file to do tomography experiments is =sim_nano_station_tomo.slx=.
|
||||
|
||||
We load the shared simulink configuration and we set the =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '5');
|
||||
#+end_src
|
||||
|
||||
We first initialize all the stages.
|
||||
|
||||
@ -15,8 +15,8 @@ open('experiment_tomography/matlab/sim_nano_station_tomo.slx')
|
||||
|
||||
% We load the shared simulink configuration and we set the =StopTime=.
|
||||
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '5');
|
||||
|
||||
|
||||
|
||||
|
||||
@ -95,14 +95,7 @@ We set the references to zero.
|
||||
|
||||
And all the controllers are set to 0.
|
||||
#+begin_src matlab
|
||||
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');
|
||||
initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
*** Identification
|
||||
@ -330,8 +323,8 @@ We initialize elements for the tomography experiment.
|
||||
|
||||
We change the simulation stop time.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '3');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '3');
|
||||
#+end_src
|
||||
|
||||
And we simulate the system.
|
||||
@ -423,14 +416,7 @@ We set the references to zero.
|
||||
|
||||
And all the controllers are set to 0.
|
||||
#+begin_src matlab
|
||||
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');
|
||||
initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
*** Identification
|
||||
|
||||
@ -74,8 +74,8 @@ The simulink file for the identification is =sim_micro_station_id.slx=.
|
||||
|
||||
We load the configuration and we set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
@ -159,8 +159,8 @@ The simulink file for the analysis is =sim_micro_station_modal_analysis.slx=.
|
||||
|
||||
We load the configuration and we set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
@ -279,12 +279,12 @@ Some of the springs and dampers values can be estimated from the joints/stages s
|
||||
|
||||
We load the configuration.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
load('mat/conf_simulink.mat');
|
||||
#+end_src
|
||||
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
|
||||
@ -152,8 +152,8 @@ We open the Simulink file.
|
||||
|
||||
We load the configuration and set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We define the wanted position/orientation of the Hexapod under study.
|
||||
|
||||
@ -89,7 +89,7 @@ The goal here is to perfectly move the station and verify that there is no misma
|
||||
** Prepare the Simulation
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
@ -232,12 +232,12 @@ We want to verify that we are able to measure this positioning error and convert
|
||||
** Prepare the Simulation
|
||||
We load the configuration.
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
load('mat/conf_simulink.mat');
|
||||
#+end_src
|
||||
|
||||
We set a small =StopTime=.
|
||||
#+begin_src matlab
|
||||
set_param(conf_simscape, 'StopTime', '0.5');
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
#+end_src
|
||||
|
||||
We initialize all the stages.
|
||||
|
||||
BIN
simscape/nass_model.slx
Normal file
BIN
simscape/nass_model.slx
Normal file
Binary file not shown.
@ -108,6 +108,69 @@ These functions are defined below.
|
||||
save('./mat/conf_simscape.mat', 'conf_simscape');
|
||||
#+end_src
|
||||
|
||||
* Logging Configuration
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeLoggingConfiguration.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeLoggingConfiguration>>
|
||||
|
||||
** Function description
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [] = initializeLoggingConfiguration(args)
|
||||
#+end_src
|
||||
|
||||
** Optional Parameters
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
args.log char {mustBeMember(args.log,{'none', 'all'})} = 'none'
|
||||
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Structure initialization
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
conf_log = struct();
|
||||
#+end_src
|
||||
|
||||
** Add Type
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
switch args.log
|
||||
case 'none'
|
||||
conf_log.type = 0;
|
||||
case 'all'
|
||||
conf_log.type = 1;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Sampling Time
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
conf_log.Ts = args.Ts;
|
||||
#+end_src
|
||||
|
||||
** Save the Structure
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
save('./mat/conf_log.mat', 'conf_log');
|
||||
#+end_src
|
||||
|
||||
* Ground
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeGround.m
|
||||
|
||||
@ -74,7 +74,7 @@ The startup script is defined below and is exported to the =project_startup.m= s
|
||||
'createDir', true);
|
||||
|
||||
%% Load the Simscape Configuration
|
||||
load('mat/conf_simscape.mat');
|
||||
load('mat/conf_simulink.mat');
|
||||
#+end_src
|
||||
|
||||
When the project closes, it runs the =project_shutdown.m= script defined below.
|
||||
|
||||
@ -15,4 +15,4 @@ Simulink.fileGenControl('set',...
|
||||
'createDir', true);
|
||||
|
||||
%% Load the Simscape Configuration
|
||||
load('mat/conf_simscape.mat');
|
||||
load('mat/conf_simulink.mat');
|
||||
|
||||
Loading…
Reference in New Issue
Block a user