408 lines
10 KiB
Matlab
408 lines
10 KiB
Matlab
%% 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]);
|