diff --git a/.SimulinkProject/Root.type.Files/simscape.type.File.xml b/.SimulinkProject/Root.type.Files/simscape.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/.SimulinkProject/Root.type.Files/simscape.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.Files/simscape.type.File/1.type.DIR_SIGNIFIER.xml b/.SimulinkProject/Root.type.Files/simscape.type.File/1.type.DIR_SIGNIFIER.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/.SimulinkProject/Root.type.Files/simscape.type.File/1.type.DIR_SIGNIFIER.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.Files/simscape.type.File/index.html.type.File.xml b/.SimulinkProject/Root.type.Files/simscape.type.File/index.html.type.File.xml new file mode 100644 index 0000000..d9a9e30 --- /dev/null +++ b/.SimulinkProject/Root.type.Files/simscape.type.File/index.html.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.Files/simscape.type.File/index.org.type.File.xml b/.SimulinkProject/Root.type.Files/simscape.type.File/index.org.type.File.xml new file mode 100644 index 0000000..1c0844e --- /dev/null +++ b/.SimulinkProject/Root.type.Files/simscape.type.File/index.org.type.File.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.Files/simscape.type.File/nass_model.slx.type.File.xml b/.SimulinkProject/Root.type.Files/simscape.type.File/nass_model.slx.type.File.xml new file mode 100644 index 0000000..80b5b16 --- /dev/null +++ b/.SimulinkProject/Root.type.Files/simscape.type.File/nass_model.slx.type.File.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/.SimulinkProject/Root.type.ProjectPath/b547b640-2f80-4b31-b2a0-09d3356191b1.type.Reference.xml b/.SimulinkProject/Root.type.ProjectPath/b547b640-2f80-4b31-b2a0-09d3356191b1.type.Reference.xml new file mode 100644 index 0000000..d2266e4 --- /dev/null +++ b/.SimulinkProject/Root.type.ProjectPath/b547b640-2f80-4b31-b2a0-09d3356191b1.type.Reference.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/active_damping/index.org b/active_damping/index.org index 33064fe..24df94a 100644 --- a/active_damping/index.org +++ b/active_damping/index.org @@ -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]]. diff --git a/active_damping/matlab/act_damp_variability_plant.m b/active_damping/matlab/act_damp_variability_plant.m index 6152d4d..caea50c 100644 --- a/active_damping/matlab/act_damp_variability_plant.m +++ b/active_damping/matlab/act_damp_variability_plant.m @@ -5,53 +5,12 @@ clear; close all; clc; s = zpk('s'); open('active_damping/matlab/sim_nass_active_damping.slx') -load('mat/conf_simscape.mat'); +load('mat/conf_simulink.mat'); -% Initialize the Simulation +% Identification :ignore: % We initialize all the stages with the default parameters. -initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); -initializeAxisc(); -initializeMirror(); - - - -% No disturbances. - -initializeDisturbances('enable', false); - - - -% The nano-hexapod is a piezoelectric hexapod. - -initializeNanoHexapod('actuator', 'piezo'); - - - -% We set the references to zero. - -initializeReferences(); - - - -% And all the controllers are set to 0. - -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'); - -% Identification -% First, we identify the dynamics of the system using the =linearize= function. +prepareLinearizeIdentification(); %% Options for Linearized options = linearizeOptions; @@ -67,7 +26,11 @@ io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = i 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; -masses = [1, 10, 50]; + + +% We identify the dynamics for the following sample mass. + +masses = [1, 10, 50]; % [kg] Gm = {zeros(length(masses))}; Gm_iff = {zeros(length(masses))}; @@ -78,7 +41,7 @@ for i = 1:length(masses) initializeSample('mass', masses(i)); %% Run the linearization - G = linearize(mdl, io, 0.1, options); + G = linearize(mdl, io, 0.3, options); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... @@ -89,11 +52,11 @@ for i = 1:length(masses) Gm_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; end -save('./active_damping/mat/plants_variable.mat', 'Gm_iff', 'Gm_dvf', 'Gm_ine', '-append'); +save('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_iff', 'Gm_dvf', 'Gm_ine', '-append'); -% Plots +% Plots :ignore: -load('./active_damping/mat/plants_variable.mat', 'Gm_iff', 'Gm_dvf', 'Gm_ine'); +load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_iff', 'Gm_dvf', 'Gm_ine'); freqs = logspace(0, 3, 1000); @@ -101,13 +64,8 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gm) - set(gca,'ColorOrderIndex',i); +for i = 1:length(Gm_iff) plot(freqs, abs(squeeze(freqresp(Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))); - for j = 2:6 - set(gca,'ColorOrderIndex',i); - plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz')))); - end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -115,13 +73,9 @@ ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Gm) - set(gca,'ColorOrderIndex',i); - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i))); - for j = 2:6 - set(gca,'ColorOrderIndex',i); - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i}(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))), 'HandleVisibility', 'off'); - end +for i = 1:length(Gm_iff) + 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'); @@ -131,11 +85,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_iff_sample_mass -% #+CAPTION: Variability of the IFF plant with the Spindle Angle ([[./figs/act_damp_variability_iff_sample_mass.png][png]], [[./figs/act_damp_variability_iff_sample_mass.pdf][pdf]]) +% #+name: fig:act_damp_variability_iff_sample_mass +% #+caption: Variability of the dynamics from actuator force to force sensor with the Sample Mass ([[./figs/act_damp_variability_iff_sample_mass.png][png]], [[./figs/act_damp_variability_iff_sample_mass.pdf][pdf]]) % [[file:figs/act_damp_variability_iff_sample_mass.png]] @@ -146,13 +101,8 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gm) - set(gca,'ColorOrderIndex',i); +for i = 1:length(Gm_dvf) plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))); - for j = 2:6 - set(gca,'ColorOrderIndex',i); - plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz')))); - end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -160,13 +110,8 @@ ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Gm) - set(gca,'ColorOrderIndex',i); +for i = 1:length(Gm_dvf) plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i))); - for j = 2:6 - set(gca,'ColorOrderIndex',i); - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_dvf{i}(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))), 'HandleVisibility', 'off'); - end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -176,11 +121,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_dvf_sample_mass -% #+CAPTION: Variability of the DVF plant with the Spindle Angle ([[./figs/act_damp_variability_dvf_sample_mass.png][png]], [[./figs/act_damp_variability_dvf_sample_mass.pdf][pdf]]) +% #+name: fig:act_damp_variability_dvf_sample_mass +% #+caption: Variability of the dynamics from actuator force to relative motion sensor with the Sample Mass ([[./figs/act_damp_variability_dvf_sample_mass.png][png]], [[./figs/act_damp_variability_dvf_sample_mass.pdf][pdf]]) % [[file:figs/act_damp_variability_dvf_sample_mass.png]] @@ -190,13 +136,8 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gm) - set(gca,'ColorOrderIndex',i); +for i = 1:length(Gm_ine) plot(freqs, abs(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz')))); - for j = 2:6 - set(gca,'ColorOrderIndex',i); - plot(freqs, abs(squeeze(freqresp(Gm_ine{i}(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz')))); - end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -204,13 +145,8 @@ ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Gm) - set(gca,'ColorOrderIndex',i); +for i = 1:length(Gm_ine) plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i))); - for j = 2:6 - set(gca,'ColorOrderIndex',i); - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_ine{i}(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))), 'HandleVisibility', 'off'); - end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); @@ -220,47 +156,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% Initialize the Simulation +% Identification :ignore: % We initialize all the stages with the default parameters. -initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); -initializeAxisc(); -initializeMirror(); - - - -% No disturbances. - -initializeDisturbances('enable', false); - - - -% The nano-hexapod is a piezoelectric hexapod. - -initializeNanoHexapod('actuator', 'piezo'); -initializeSample('mass', 50); - - - -% And all the controllers are set to 0. - -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'); - -% Identification -% First, we identify the dynamics of the system using the =linearize= function. +prepareLinearizeIdentification(); %% Options for Linearized options = linearizeOptions; @@ -276,6 +177,10 @@ io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = i 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; + + +% We identify the dynamics for the following Spindle angles. + Rz_amplitudes = [0, pi/4, pi/2, pi]; % [rad] Ga = {zeros(length(Rz_amplitudes))}; @@ -284,10 +189,10 @@ Ga_dvf = {zeros(length(Rz_amplitudes))}; Ga_ine = {zeros(length(Rz_amplitudes))}; for i = 1:length(Rz_amplitudes) -initializeReferences('Rz_type', 'constant', 'Rz_amplitude', Rz_amplitudes(i)) + initializeReferences('Rz_type', 'constant', 'Rz_amplitude', Rz_amplitudes(i)) %% Run the linearization - G = linearize(mdl, io, 0.1, options); + G = linearize(mdl, io, 0.3, options); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... @@ -298,11 +203,11 @@ initializeReferences('Rz_type', 'constant', 'Rz_amplitude', Rz_amplitudes(i)) Ga_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; end -save('./active_damping/mat/plants_variable.mat', 'Ga_iff', 'Ga_dvf', 'Ga_ine', '-append'); +save('./active_damping/mat/plants_variable.mat', 'Rz_amplitudes', 'Ga_iff', 'Ga_dvf', 'Ga_ine', '-append'); -% Plots +% Plots :ignore: -load('./active_damping/mat/plants_variable.mat', 'Ga_iff', 'Ga_dvf', 'Ga_ine'); +load('./active_damping/mat/plants_variable.mat', 'Rz_amplitudes', 'Ga_iff', 'Ga_dvf', 'Ga_ine'); freqs = logspace(0, 3, 1000); @@ -310,7 +215,7 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Ga) +for i = 1:length(Ga_iff) plot(freqs, abs(squeeze(freqresp(Ga_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))); end hold off; @@ -319,7 +224,7 @@ ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Ga) +for i = 1:length(Ga_iff) plot(freqs, 180/pi*angle(squeeze(freqresp(Ga_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [deg]', Rz_amplitudes(i)*180/pi)); end hold off; @@ -330,11 +235,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_iff_spindle_angle -% #+CAPTION: Variability of the IFF plant with the Spindle Angle ([[./figs/act_damp_variability_iff_spindle_angle.png][png]], [[./figs/act_damp_variability_iff_spindle_angle.pdf][pdf]]) +% #+name: fig:act_damp_variability_iff_spindle_angle +% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Spindle Angle ([[./figs/act_damp_variability_iff_spindle_angle.png][png]], [[./figs/act_damp_variability_iff_spindle_angle.pdf][pdf]]) % [[file:figs/act_damp_variability_iff_spindle_angle.png]] @@ -345,7 +251,7 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Ga) +for i = 1:length(Ga_dvf) plot(freqs, abs(squeeze(freqresp(Ga_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))); end hold off; @@ -354,7 +260,7 @@ ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Ga) +for i = 1:length(Ga_dvf) plot(freqs, 180/pi*angle(squeeze(freqresp(Ga_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [deg]', Rz_amplitudes(i)*180/pi)); end hold off; @@ -365,11 +271,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_dvf_spindle_angle -% #+CAPTION: Variability of the DVF plant with the Spindle Angle ([[./figs/act_damp_variability_dvf_spindle_angle.png][png]], [[./figs/act_damp_variability_dvf_spindle_angle.pdf][pdf]]) +% #+name: fig:act_damp_variability_dvf_spindle_angle +% #+caption: Variability of the dynamics from actuator force to relative motion sensor with the Spindle Angle ([[./figs/act_damp_variability_dvf_spindle_angle.png][png]], [[./figs/act_damp_variability_dvf_spindle_angle.pdf][pdf]]) % [[file:figs/act_damp_variability_dvf_spindle_angle.png]] @@ -379,7 +286,7 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Ga) +for i = 1:length(Ga_ine) plot(freqs, abs(squeeze(freqresp(Ga_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz')))); end hold off; @@ -388,7 +295,7 @@ ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Ga) +for i = 1:length(Ga_ine) plot(freqs, 180/pi*angle(squeeze(freqresp(Ga_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [deg]', Rz_amplitudes(i)*180/pi)); end hold off; @@ -399,47 +306,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% Initialize the Simulation +% Identification :ignore: % We initialize all the stages with the default parameters. -initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); -initializeAxisc(); -initializeMirror(); - - - -% No disturbances. - -initializeDisturbances('enable', false); - - - -% The nano-hexapod is a piezoelectric hexapod. - -initializeNanoHexapod('actuator', 'piezo'); -initializeSample('mass', 50); - - - -% And all the controllers are set to 0. - -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'); - -% Identification -% First, we identify the dynamics of the system using the =linearize= function. +prepareLinearizeIdentification(); %% Options for Linearized options = linearizeOptions; @@ -455,7 +327,16 @@ io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = i 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; -Rz_periods = [60, 10, 1]; % [s] + + +% We identify the dynamics for the following Spindle rotation periods. + +Rz_periods = [60, 6, 2, 1]; % [s] + + + +% The identification of the dynamics is done at the same Spindle angle position. + Gw = {zeros(length(Rz_periods))}; Gw_iff = {zeros(length(Rz_periods))}; @@ -463,44 +344,54 @@ Gw_dvf = {zeros(length(Rz_periods))}; Gw_ine = {zeros(length(Rz_periods))}; for i = 1:length(Rz_periods) - initializeReferences('Rz_type', 'rotating', 'Rz_period', Rz_periods(i)); + initializeReferences('Rz_type', 'rotating', ... + 'Rz_period', Rz_periods(i), ... % Rotation period [s] + 'Rz_amplitude', -0.5*(2*pi/Rz_periods(i))); % Angle offset [rad] + + load('mat/nass_references.mat', 'Rz'); % We load the reference for the Spindle + [~, i_end] = min(abs(Rz.signals.values)); % Obtain the indice where the spindle angle is zero + t_sim = Rz.time(i_end) % Simulation time before identification [s] %% Run the linearization - G = linearize(mdl, io, 0.5, options); + G = linearize(mdl, io, t_sim, options); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... 'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}; + Gw(i) = {G}; Gw_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; Gw_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; Gw_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; end -save('./active_damping/mat/plants_variable.mat', 'Gw_iff', 'Gw_dvf', 'Gw_ine', '-append'); +save('./active_damping/mat/plants_variable.mat', 'Rz_periods', 'Gw_iff', 'Gw_dvf', 'Gw_ine', '-append'); -% Plots +% Dynamics of the Active Damping plants -load('./active_damping/mat/plants_variable.mat', 'Gw_iff', 'Gw_dvf', 'Gw_ine'); +load('./active_damping/mat/plants_variable.mat', 'Rz_periods', 'Gw_iff', 'Gw_dvf', 'Gw_ine'); +load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine'); -freqs = logspace(0, 3, 1000); +freqs = logspace(0, 3, 10000); figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gw) +for i = 1:length(Gw_iff) plot(freqs, abs(squeeze(freqresp(Gw_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))); end +plot(freqs, abs(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); 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:length(Gw) +for i = 1:length(Gw_iff) plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(i))); end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', 'No Rotation'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); @@ -509,33 +400,45 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_iff_spindle_speed -% #+CAPTION: Variability of the IFF plant with the Spindle Angle ([[./figs/act_damp_variability_iff_spindle_speed.png][png]], [[./figs/act_damp_variability_iff_spindle_speed.pdf][pdf]]) +% #+name: fig:act_damp_variability_iff_spindle_speed +% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Spindle rotation speed ([[./figs/act_damp_variability_iff_spindle_speed.png][png]], [[./figs/act_damp_variability_iff_spindle_speed.pdf][pdf]]) % [[file:figs/act_damp_variability_iff_spindle_speed.png]] -freqs = logspace(0, 3, 1000); +xlim([20, 30]); + + + +% #+name: fig:act_damp_variability_iff_spindle_speed_zoom +% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Spindle rotation speed ([[./figs/act_damp_variability_iff_spindle_speed_zoom.png][png]], [[./figs/act_damp_variability_iff_spindle_speed_zoom.pdf][pdf]]) +% [[file:figs/act_damp_variability_iff_spindle_speed_zoom.png]] + + +freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gw) +for i = 1:length(Gw_dvf) plot(freqs, abs(squeeze(freqresp(Gw_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))); end +plot(freqs, abs(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Gw) +for i = 1:length(Gw_dvf) plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(i))); end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', 'No Rotation'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); @@ -544,32 +447,44 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_dvf_spindle_speed -% #+CAPTION: Variability of the DVF plant with the Spindle Angle ([[./figs/act_damp_variability_dvf_spindle_speed.png][png]], [[./figs/act_damp_variability_dvf_spindle_speed.pdf][pdf]]) +% #+name: fig:act_damp_variability_dvf_spindle_speed +% #+caption: Variability of the dynamics from the actuator force to the relative motion sensor with the Spindle rotation speed ([[./figs/act_damp_variability_dvf_spindle_speed.png][png]], [[./figs/act_damp_variability_dvf_spindle_speed.pdf][pdf]]) % [[file:figs/act_damp_variability_dvf_spindle_speed.png]] -freqs = logspace(0, 2, 5000); +xlim([20, 30]); + + + +% #+name: fig:act_damp_variability_dvf_spindle_speed_zoom +% #+caption: Variability of the dynamics from the actuator force to the relative motion sensor with the Spindle rotation speed ([[./figs/act_damp_variability_dvf_spindle_speed_zoom.png][png]], [[./figs/act_damp_variability_dvf_spindle_speed_zoom.pdf][pdf]]) +% [[file:figs/act_damp_variability_dvf_spindle_speed_zoom.png]] + + +freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gw) +for i = 1:length(Gw_ine) plot(freqs, abs(squeeze(freqresp(Gw_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz')))); end +plot(freqs, abs(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Gw) +for i = 1:length(Gw_ine) plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(i))); end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', 'No Rotation'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); @@ -578,47 +493,102 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% Initialize the Simulation + + +% #+name: fig:act_damp_variability_ine_spindle_speed +% #+caption: Variability of the dynamics from the actuator force to the absolute velocity sensor with the Spindle rotation speed ([[./figs/act_damp_variability_ine_spindle_speed.png][png]], [[./figs/act_damp_variability_ine_spindle_speed.pdf][pdf]]) +% [[file:figs/act_damp_variability_ine_spindle_speed.png]] + + +xlim([20, 30]); + +% Variation of the poles and zeros with the Spindle rotation frequency + +load('./active_damping/mat/plants_variable.mat', 'Rz_periods', 'Gw_iff', 'Gw_dvf', 'Gw_ine'); +load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine'); + +figure; + +subplot(1,2,1); +hold on; +for i = 1:length(Gw_iff) + G_poles = pole(Gw_iff{i}('Fnlm1', 'Fnl1')); + plot(1/Rz_periods(i), real(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx'); +end +G_poles = pole(G_iff('Fnlm1', 'Fnl1')); +plot(0, real(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx'); +hold off; +ylim([-inf, 0]); +xlabel('Rotation Speed [Hz]'); +ylabel('Real Part'); + +subplot(1,2,2); +hold on; +for i = 1:length(Gw_iff) + G_poles = pole(Gw_iff{i}('Fnlm1', 'Fnl1')); + plot(1/Rz_periods(i), imag(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx'); +end +G_poles = pole(G_iff('Fnlm1', 'Fnl1')); +plot(0, imag(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx'); +hold off; +ylim([0, inf]); +xlabel('Rotation Speed [Hz]'); +ylabel('Imaginary Part'); + + + +% #+name: fig:campbell_diagram_spindle_rotation +% #+caption: Evolution of the pole with respect to the spindle rotation speed ([[./figs/campbell_diagram_spindle_rotation.png][png]], [[./figs/campbell_diagram_spindle_rotation.pdf][pdf]]) +% [[file:figs/campbell_diagram_spindle_rotation.png]] + + +figure; + +subplot(1,2,1); +hold on; +for i = 1:length(Gw_ine) + set(gca,'ColorOrderIndex',1); + G_zeros = zero(Gw_ine{i}('Vnlm1', 'Fnl1')); + plot(1/Rz_periods(i), real(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o'); + + set(gca,'ColorOrderIndex',2); + G_zeros = zero(Gw_iff{i}('Fnlm1', 'Fnl1')); + plot(1/Rz_periods(i), real(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o'); + + set(gca,'ColorOrderIndex',3); + G_zeros = zero(Gw_dvf{i}('Dnlm1', 'Fnl1')); + plot(1/Rz_periods(i), real(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o'); +end +hold off; +xlabel('Rotation Speed [Hz]'); +ylabel('Real Part'); + +subplot(1,2,2); +hold on; +for i = 1:length(Gw_ine) + set(gca,'ColorOrderIndex',1); + G_zeros = zero(Gw_ine{i}('Vnlm1', 'Fnl1')); + p_ine = plot(1/Rz_periods(i), imag(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o'); + + set(gca,'ColorOrderIndex',2); + G_zeros = zero(Gw_iff{i}('Fnlm1', 'Fnl1')); + p_iff = plot(1/Rz_periods(i), imag(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o'); + + set(gca,'ColorOrderIndex',3); + G_zeros = zero(Gw_dvf{i}('Dnlm1', 'Fnl1')); + p_dvf = plot(1/Rz_periods(i), imag(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o'); +end +hold off; +xlabel('Rotation Speed [Hz]'); +ylabel('Imaginary Part'); +legend([p_ine p_iff p_dvf],{'Inertial Sensor','Force Sensor', 'Relative Motion Sensor'}, 'location', 'southwest'); + +% Identification :ignore: % We initialize all the stages with the default parameters. -initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); -initializeAxisc(); -initializeMirror(); - - - -% No disturbances. - -initializeDisturbances('enable', false); - - - -% The nano-hexapod is a piezoelectric hexapod. - -initializeNanoHexapod('actuator', 'piezo'); -initializeSample('mass', 50); - - - -% And all the controllers are set to 0. - -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'); - -% Identification -% First, we identify the dynamics of the system using the =linearize= function. +prepareLinearizeIdentification(); %% Options for Linearized options = linearizeOptions; @@ -634,7 +604,11 @@ io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = i 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; -Ry_amplitudes = [0, 3*pi/180]; % [rad] + + +% We identify the dynamics for the following Tilt stage angles. + +Ry_amplitudes = [-3*pi/180, 3*pi/180]; % [rad] Gy = {zeros(length(Ry_amplitudes))}; Gy_iff = {zeros(length(Ry_amplitudes))}; @@ -645,7 +619,7 @@ for i = 1:length(Ry_amplitudes) initializeReferences('Ry_type', 'constant', 'Ry_amplitude', Ry_amplitudes(i)) %% Run the linearization - G = linearize(mdl, io, 0.1, options); + G = linearize(mdl, io, 0.3, options); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... @@ -656,11 +630,12 @@ for i = 1:length(Ry_amplitudes) Gy_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; end -save('./active_damping/mat/plants_variable.mat', 'Gy_iff', 'Gy_dvf', 'Gy_ine', '-append'); +save('./active_damping/mat/plants_variable.mat', 'Ry_amplitudes', 'Gy_iff', 'Gy_dvf', 'Gy_ine', '-append'); -% Plots +% Plots :ignore: -load('./active_damping/mat/plants_variable.mat', 'Gy_iff', 'Gy_dvf', 'Gy_ine'); +load('./active_damping/mat/plants_variable.mat', 'Ry_amplitudes', 'Gy_iff', 'Gy_dvf', 'Gy_ine'); +load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine'); freqs = logspace(0, 3, 1000); @@ -668,18 +643,20 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gy) +for i = 1:length(Gy_iff) plot(freqs, abs(squeeze(freqresp(Gy_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))); end +plot(freqs, abs(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); 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:length(Gy) +for i = 1:length(Gy_iff) plot(freqs, 180/pi*angle(squeeze(freqresp(Gy_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Ry = %.0f$ [deg]', Ry_amplitudes(i)*180/pi)); end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); @@ -688,11 +665,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_iff_tilt_angle -% #+CAPTION: Variability of the IFF plant with the Spindle Angle ([[./figs/act_damp_variability_iff_tilt_angle.png][png]], [[./figs/act_damp_variability_iff_tilt_angle.pdf][pdf]]) +% #+name: fig:act_damp_variability_iff_tilt_angle +% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Tilt stage Angle ([[./figs/act_damp_variability_iff_tilt_angle.png][png]], [[./figs/act_damp_variability_iff_tilt_angle.pdf][pdf]]) % [[file:figs/act_damp_variability_iff_tilt_angle.png]] @@ -703,18 +681,20 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gy) +for i = 1:length(Gy_dvf) plot(freqs, abs(squeeze(freqresp(Gy_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))); end +plot(freqs, abs(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Gy) +for i = 1:length(Gy_dvf) plot(freqs, 180/pi*angle(squeeze(freqresp(Gy_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Ry = %.0f$ [deg]', Ry_amplitudes(i)*180/pi)); end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); @@ -723,11 +703,12 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); -% #+NAME: fig:act_damp_variability_dvf_tilt_angle -% #+CAPTION: Variability of the DVF plant with the Spindle Angle ([[./figs/act_damp_variability_dvf_tilt_angle.png][png]], [[./figs/act_damp_variability_dvf_tilt_angle.pdf][pdf]]) +% #+name: fig:act_damp_variability_dvf_tilt_angle +% #+caption: Variability of the dynamics from the actuator force to the relative motion sensor with the Tilt Angle ([[./figs/act_damp_variability_dvf_tilt_angle.png][png]], [[./figs/act_damp_variability_dvf_tilt_angle.pdf][pdf]]) % [[file:figs/act_damp_variability_dvf_tilt_angle.png]] @@ -737,18 +718,20 @@ figure; ax1 = subplot(2, 1, 1); hold on; -for i = 1:length(Gy) +for i = 1:length(Gy_ine) plot(freqs, abs(squeeze(freqresp(Gy_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz')))); end +plot(freqs, abs(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; -for i = 1:length(Gy) +for i = 1:length(Gy_ine) plot(freqs, 180/pi*angle(squeeze(freqresp(Gy_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Ry = %.0f$ [deg]', Ry_amplitudes(i)*180/pi)); end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); @@ -757,3 +740,182 @@ yticks([-180, -90, 0, 90, 180]); legend('location', 'southwest'); linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Identification :ignore: +% We initialize all the stages with the default parameters. + +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; +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; + + + +% We initialize the translation stage reference to be a sinus with an amplitude of 5mm and a period of 1s (Figure [[fig:ty_scanning_reference_sinus]]). + +initializeReferences('Dy_type', 'sinusoidal', ... + 'Dy_amplitude', 5e-3, ... % [m] + 'Dy_period', 1); % [s] + +load('mat/nass_references.mat', 'Dy'); +figure; +plot(Dy.time, Dy.signals.values); +xlabel('Time [s]'); ylabel('Dy - Position [m]'); +xlim([0, 2]); + + + +% #+name: fig:ty_scanning_reference_sinus +% #+caption: Reference path for the translation stage ([[./figs/ty_scanning_reference_sinus.png][png]], [[./figs/ty_scanning_reference_sinus.pdf][pdf]]) +% [[file:figs/ty_scanning_reference_sinus.png]] + +% We identify the dynamics at different positions (times) when scanning with the Translation stage. + +t_lin = [0.5, 0.75, 1, 1.25]; + +Gty = {zeros(length(t_lin))}; +Gty_iff = {zeros(length(t_lin))}; +Gty_dvf = {zeros(length(t_lin))}; +Gty_ine = {zeros(length(t_lin))}; + +%% Run the linearization +G = linearize(mdl, io, t_lin, options); +G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; +G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}; + +for i = 1:length(t_lin) + Gty(i) = {G(:,:,i)}; + Gty_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}, i))}; + Gty_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}, i))}; + Gty_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}, i))}; +end + +Gty_tlin = t_lin; +save('./active_damping/mat/plants_variable.mat', 'Gty_tlin', 'Dy', 'Gty_iff', 'Gty_dvf', 'Gty_ine', '-append'); + +% Plots :ignore: + +load('./active_damping/mat/plants_variable.mat', 'Gty_tlin', 'Dy', 'Gty_iff', 'Gty_dvf', 'Gty_ine'); +load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine'); + +freqs = logspace(0, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Gty_iff) + plot(freqs, abs(squeeze(freqresp(Gty_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))); +end +plot(freqs, abs(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); +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:length(Gty_iff) + [~, i_t] = min(abs(Dy.time - Gty_tlin(i))); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gty_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Dy = %.0f$ [mm]', 1e3*Dy.signals.values(i_t))); +end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]'); +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'); +xlim([freqs(1), freqs(end)]); + + + +% #+name: fig:act_damp_variability_iff_ty_scans +% #+caption: Variability of the dynamics from the actuator force to the absolute velocity sensor plant at different Ty scan positions ([[./figs/act_damp_variability_iff_ty_scans.png][png]], [[./figs/act_damp_variability_iff_ty_scans.pdf][pdf]]) +% [[file:figs/act_damp_variability_iff_ty_scans.png]] + + +freqs = logspace(0, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; + +for i = 1:length(Gty_dvf) + plot(freqs, abs(squeeze(freqresp(Gty_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))); +end +plot(freqs, abs(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Gty_dvf) + [~, i_t] = min(abs(Dy.time - Gty_tlin(i))); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gty_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Dy = %.0f$ [mm]', 1e3*Dy.signals.values(i_t))); +end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]'); +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'); +xlim([freqs(1), freqs(end)]); + + + +% #+name: fig:act_damp_variability_dvf_ty_scans +% #+caption: Variability of the dynamics from actuator force to relative displacement sensor at different Ty scan positions ([[./figs/act_damp_variability_dvf_ty_scans.png][png]], [[./figs/act_damp_variability_dvf_ty_scans.pdf][pdf]]) +% [[file:figs/act_damp_variability_dvf_ty_scans.png]] + + +freqs = logspace(0, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Gty_ine) + plot(freqs, abs(squeeze(freqresp(Gty_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz')))); +end +plot(freqs, abs(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Gty_ine) + [~, i_t] = min(abs(Dy.time - Gty_tlin(i))); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gty_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Dy = %.0f$ [mm]', 1e3*Dy.signals.values(i_t))); +end +plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]'); +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'); +xlim([freqs(1), freqs(end)]); diff --git a/active_damping/matlab/dvf.m b/active_damping/matlab/dvf.m index 16df905..95915c6 100644 --- a/active_damping/matlab/dvf.m +++ b/active_damping/matlab/dvf.m @@ -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]); diff --git a/active_damping/matlab/iff.m b/active_damping/matlab/iff.m index c238bd3..fe07927 100644 --- a/active_damping/matlab/iff.m +++ b/active_damping/matlab/iff.m @@ -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]); diff --git a/active_damping/matlab/ine.m b/active_damping/matlab/ine.m index 4c6e624..6134903 100644 --- a/active_damping/matlab/ine.m +++ b/active_damping/matlab/ine.m @@ -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 diff --git a/active_damping/matlab/undamped_system.m b/active_damping/matlab/undamped_system.m index e49889d..32c176d 100644 --- a/active_damping/matlab/undamped_system.m +++ b/active_damping/matlab/undamped_system.m @@ -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]); diff --git a/active_damping/src/prepareLinearizeIdentification.m b/active_damping/src/prepareLinearizeIdentification.m index 9b3e555..3488e7f 100644 --- a/active_damping/src/prepareLinearizeIdentification.m +++ b/active_damping/src/prepareLinearizeIdentification.m @@ -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'); diff --git a/active_damping/src/prepareTomographyExperiment.m b/active_damping/src/prepareTomographyExperiment.m index 7a65466..441044d 100644 --- a/active_damping/src/prepareTomographyExperiment.m +++ b/active_damping/src/prepareTomographyExperiment.m @@ -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'); diff --git a/disturbances/index.org b/disturbances/index.org index a9c4023..9da2f92 100644 --- a/disturbances/index.org +++ b/disturbances/index.org @@ -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. diff --git a/experiment_tomography/index.org b/experiment_tomography/index.org index c6b4932..c83d6ce 100644 --- a/experiment_tomography/index.org +++ b/experiment_tomography/index.org @@ -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. diff --git a/experiment_tomography/matlab/tomo_exp.m b/experiment_tomography/matlab/tomo_exp.m index a6df3dd..aa2ca64 100644 --- a/experiment_tomography/matlab/tomo_exp.m +++ b/experiment_tomography/matlab/tomo_exp.m @@ -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'); diff --git a/hac_lac/index.org b/hac_lac/index.org index e4f3f5a..c45b262 100644 --- a/hac_lac/index.org +++ b/hac_lac/index.org @@ -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 diff --git a/identification/index.org b/identification/index.org index 97dfc23..fdc4c6a 100644 --- a/identification/index.org +++ b/identification/index.org @@ -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. diff --git a/kinematics/index.org b/kinematics/index.org index b78a493..3c755ef 100644 --- a/kinematics/index.org +++ b/kinematics/index.org @@ -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. diff --git a/positioning_error/index.org b/positioning_error/index.org index 2f819a2..2efbb39 100644 --- a/positioning_error/index.org +++ b/positioning_error/index.org @@ -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. diff --git a/simscape/nass_model.slx b/simscape/nass_model.slx new file mode 100644 index 0000000..018390b Binary files /dev/null and b/simscape/nass_model.slx differ diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org index d8911e7..18ffef6 100644 --- a/simscape_subsystems/index.org +++ b/simscape_subsystems/index.org @@ -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: +<> + +** 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 diff --git a/simulink_project/index.org b/simulink_project/index.org index 9d3ce3c..82f6586 100644 --- a/simulink_project/index.org +++ b/simulink_project/index.org @@ -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. diff --git a/src/project_startup.m b/src/project_startup.m index 0cca35e..c635e75 100644 --- a/src/project_startup.m +++ b/src/project_startup.m @@ -15,4 +15,4 @@ Simulink.fileGenControl('set',... 'createDir', true); %% Load the Simscape Configuration -load('mat/conf_simscape.mat'); +load('mat/conf_simulink.mat');