tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Change all the organization of the files

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-02-25 18:10:20 +01:00
parent abe29e9938
commit ceefaa91a3
518 changed files with 7749 additions and 6437 deletions
Download Patch File Download Diff File Expand all files Collapse all files

921
matlab/act_damp_variability_plant.m Normal file
View File

@@ -0,0 +1,921 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
open('active_damping/matlab/sim_nass_active_damping.slx')
load('mat/conf_simulink.mat');
% 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 identify the dynamics for the following sample mass.
masses = [1, 10, 50]; % [kg]
Gm = {zeros(length(masses))};
Gm_iff = {zeros(length(masses))};
Gm_dvf = {zeros(length(masses))};
Gm_ine = {zeros(length(masses))};
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 = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
Gm(i) = {G};
Gm_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
Gm_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
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', 'masses', 'Gm_iff', 'Gm_dvf', 'Gm_ine', '-append');
% Plots :ignore:
load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_iff', 'Gm_dvf', 'Gm_ine');
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gm_iff)
plot(freqs, abs(squeeze(freqresp(Gm_iff{i}('Fnlm1', 'Fnl1'), 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: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');
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_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]]
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gm_dvf)
plot(freqs, abs(squeeze(freqresp(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',[]);
ax2 = subplot(2, 1, 2);
hold on;
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)));
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');
xlim([freqs(1), freqs(end)]);
% #+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]]
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gm_ine)
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 [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
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)));
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');
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 identify the dynamics for the following Spindle angles.
Rz_amplitudes = [0, pi/4, pi/2, pi]; % [rad]
Ga = {zeros(length(Rz_amplitudes))};
Ga_iff = {zeros(length(Rz_amplitudes))};
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))
%% Run the linearization
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', ...
'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
Ga(i) = {G};
Ga_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
Ga_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
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', 'Rz_amplitudes', 'Ga_iff', 'Ga_dvf', 'Ga_ine', '-append');
% Plots :ignore:
load('./active_damping/mat/plants_variable.mat', 'Rz_amplitudes', 'Ga_iff', 'Ga_dvf', 'Ga_ine');
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ga_iff)
plot(freqs, abs(squeeze(freqresp(Ga_iff{i}('Fnlm1', 'Fnl1'), 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: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;
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_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]]
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ga_dvf)
plot(freqs, abs(squeeze(freqresp(Ga_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1: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;
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_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]]
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ga_ine)
plot(freqs, abs(squeeze(freqresp(Ga_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
end
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(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;
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)]);
% 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 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))};
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), ... % 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, 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', 'Rz_periods', 'Gw_iff', 'Gw_dvf', 'Gw_ine', '-append');
% Dynamics of the Active Damping plants
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, 10000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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_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]');
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_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]]
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_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_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]');
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_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]]
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_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_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]');
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_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.
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 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))};
Gy_dvf = {zeros(length(Ry_amplitudes))};
Gy_ine = {zeros(length(Ry_amplitudes))};
for i = 1:length(Ry_amplitudes)
initializeReferences('Ry_type', 'constant', 'Ry_amplitude', Ry_amplitudes(i))
%% Run the linearization
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', ...
'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
Gy(i) = {G};
Gy_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
Gy_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
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', 'Ry_amplitudes', 'Gy_iff', 'Gy_dvf', 'Gy_ine', '-append');
% Plots :ignore:
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);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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_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]');
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_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]]
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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_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]');
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_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]]
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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_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]');
ylim([-180, 180]);
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)]);

279
matlab/dvf.m Normal file
View File

@@ -0,0 +1,279 @@
%% Clear Workspace and Close figures
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_dvf');
load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_dvf');
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
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*30000/(1 + s/2/pi/10000);
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
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');
K_dvf = -K_dvf*eye(6);
save('./active_damping/mat/K_dvf.mat', 'K_dvf');
prepareLinearizeIdentification();
load('./active_damping/mat/K_dvf.mat', 'K_dvf');
initializeController('type', 'dvf', 'K', K_dvf);
%% 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, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
load('./active_damping/mat/cart_plants.mat', 'masses');
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;
ax1 = subplot(2, 1, 1);
hold on;
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([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}('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]');
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: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]');
yticks([-540:180:540]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
freqs = logspace(1, 3, 1000);
figure;
for ix = 1:6
for iy = 1:6
subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(G_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
prepareTomographyExperiment();
load('./active_damping/mat/K_dvf.mat', 'K_dvf');
initializeController('type', 'dvf', 'K', K_dvf);
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '4.5');
sim('sim_nass_active_damping');
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', 'En', 'En_dvf');
Fs = 1e3; % Sampling Frequency of the Data
t = (1/Fs)*[0:length(En(:,1))-1];
figure;
hold on;
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, 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]);

280
matlab/iff.m Normal file
View File

@@ -0,0 +1,280 @@
%% Clear Workspace and Close figures
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(-2, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
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');
w0 = 2*pi*50;
K_iff = -5000/s * (s/w0)/(1 + s/w0);
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
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');
K_iff = -K_iff*eye(6);
save('./active_damping/mat/K_iff.mat', 'K_iff');
prepareLinearizeIdentification();
load('./active_damping/mat/K_iff.mat', 'K_iff');
initializeController('type', 'iff', 'K', K_iff);
%% 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, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
load('./active_damping/mat/cart_plants.mat', 'masses');
G_cart_iff = {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_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;
ax1 = subplot(2, 1, 1);
hold on;
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([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}('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]');
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: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]');
yticks([-540:180:540]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
freqs = logspace(1, 3, 1000);
figure;
for ix = 1:6
for iy = 1:6
subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(G_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-13, 1e-4]);
xticks([])
yticks([])
end
end
prepareTomographyExperiment();
load('./active_damping/mat/K_iff.mat', 'K_iff');
initializeController('type', 'iff', 'K', K_iff);
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '4.5');
sim('sim_nass_active_damping');
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', 'En', 'En_iff');
Fs = 1e3; % Sampling Frequency of the Data
t = (1/Fs)*[0:length(En(:,1))-1];
figure;
hold on;
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, 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]);

206
matlab/ine.m Normal file
View File

@@ -0,0 +1,206 @@
%% Clear Workspace and Close figures
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);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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/s)/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
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 = 2.5e4;
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
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('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
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');
K_ine = -K_ine*eye(6);
save('./active_damping/mat/K_ine.mat', 'K_ine');
prepareLinearizeIdentification();
load('./active_damping/mat/K_ine.mat', 'K_ine');
initializeController('type', 'ine', 'K', K_ine);
%% 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, '/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;
ax1 = subplot(2, 1, 1);
hold on;
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([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}('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]');
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: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]');
yticks([-540:180:540]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
freqs = logspace(1, 3, 1000);
figure;
for ix = 1:6
for iy = 1:6
subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(G_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

BIN
matlab/nass_model.slx Normal file
View File

Binary file not shown.

407
matlab/tomo_exp.m Normal file
View File

@@ -0,0 +1,407 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Simscape Model
% <<sec:simscape_model>>
open('nass_model.slx');
% We load the shared simulink configuration and we set the =StopTime=.
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '5');
% We first initialize all the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 1);
% We initialize the reference path for all the stages.
% All stage is set to its zero position except the Spindle which is rotating at 60rpm.
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
% Simulation Setup
% And we initialize the disturbances to be equal to zero.
initializeDisturbances(...
'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction
'Fty_x', false, ... % Translation Stage - X direction
'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
tomo_align_no_dist = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist');
t = tomo_align_no_dist.t;
MTr = tomo_align_no_dist.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_tomo_without_dist_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_trans.png][png]], [[./figs/exp_tomo_without_dist_trans.pdf][pdf]])
% [[file:figs/exp_tomo_without_dist_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% Simulation Setup
% We now activate the disturbances.
initializeDisturbances(...
'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction
'Fty_x', true, ... % Translation Stage - X direction
'Fty_z', true, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
tomo_align_dist = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_align_dist', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'tomo_align_dist');
t = tomo_align_dist.t;
MTr = tomo_align_dist.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_tomo_dist_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_trans.png][png]], [[./figs/exp_tomo_dist_trans.pdf][pdf]])
% [[file:figs/exp_tomo_dist_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% Simulation Setup
% We first set the wanted translation of the Micro Hexapod.
P_micro_hexapod = [0.01; 0; 0]; % [m]
% We initialize the reference path.
initializeReferences('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1);
% We initialize the stages.
initializeMicroHexapod('AP', P_micro_hexapod);
% And we initialize the disturbances to zero.
initializeDisturbances(...
'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction
'Fty_x', false, ... % Translation Stage - X direction
'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
tomo_not_align = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_not_align', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'tomo_not_align');
t = tomo_not_align.t;
MTr = tomo_not_align.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_tomo_offset_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_offset_trans.png][png]], [[./figs/exp_tomo_offset_trans.pdf][pdf]])
% [[file:figs/exp_tomo_offset_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% Simulation Setup
% We set the reference path.
initializeReferences('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1);
% We initialize the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 1);
% And we initialize the disturbances to zero.
initializeDisturbances(...
'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction
'Fty_x', false, ... % Translation Stage - X direction
'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
ty_scan = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'ty_scan', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'ty_scan');
t = ty_scan.t;
MTr = ty_scan.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_ty_scan_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_ty_scan_trans.png][png]], [[./figs/exp_ty_scan_trans.pdf][pdf]])
% [[file:figs/exp_ty_scan_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);

285
matlab/undamped_system.m Normal file
View File

@@ -0,0 +1,285 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
addpath('active_damping/src/');
open('active_damping/matlab/sim_nass_active_damping.slx')
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, '/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, 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', ...
'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
G_dvf = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
G_ine = minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
save('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');
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:6
plot(freqs, abs(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\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_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
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: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
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
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: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]');
yticks([-540:180:540]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
prepareTomographyExperiment();
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '4.5');
sim('sim_nass_active_damping');
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, 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, 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]');
linkaxes([ax1,ax2,ax3],'x');
xlim([0.5,inf]);