nass-simscape/experiment_tomography/matlab/tomo_exp.m

413 lines
11 KiB
Matlab

%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Simscape Model
% <<sec:simscape_model>>
% The simulink file to do tomography experiments is =sim_nano_station_tomo.slx=.
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');
% We first initialize all the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('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(struct('Rz_type', 'rotating', 'Rz_period', 1));
% Simulation Setup
% And we initialize the disturbances to be equal to zero.
opts = struct(...
'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
);
initDisturbances(opts);
% We simulate the model.
sim('sim_nano_station_tomo');
% 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([1, 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([1, inf]);
% Simulation Setup
% We now activate the disturbances.
opts = struct(...
'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
);
initDisturbances(opts);
% We simulate the model.
sim('sim_nano_station_tomo');
% 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([1, 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([1, 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(struct('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1));
% We initialize the stages.
initializeMicroHexapod(struct('AP', P_micro_hexapod));
% And we initialize the disturbances to zero.
opts = struct(...
'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
);
initDisturbances(opts);
% We simulate the model.
sim('sim_nano_station_tomo');
% 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([1, 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([1, inf]);
% Simulation Setup
% We set the reference path.
initializeReferences(struct('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1));
% We initialize the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 1));
% And we initialize the disturbances to zero.
opts = struct(...
'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
);
initDisturbances(opts);
% We simulate the model.
sim('sim_nano_station_tomo');
% 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([1, 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([1, inf]);