% Matlab Init                                              :noexport:ignore:

%% ustation_4_experiments.m

%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

% Simulink Model name
mdl = 'ustation_simscape';

load('nass_model_conf_simulink.mat');

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);

% Tomography Experiment
% <<sec:ustation_experiments_tomography>>

% To simulate a tomography experiment, the setpoint of the Spindle is configured to perform a constant rotation with a rotational velocity of 60rpm.
% Both ground motion and spindle vibration disturbances were simulated based on what was computed in Section ref:sec:ustation_disturbances.
% A radial offset of $\approx 1\,\mu m$ between the "point-of-interest" and the spindle's rotation axis is introduced to represent what is experimentally observed.
% During the 10 second simulation (i.e. 10 spindle turns), the position of the "point-of-interest" with respect to the granite was recorded.
% Results are shown in Figure ref:fig:ustation_errors_model_spindle.
% A good correlation with the measurements is observed both for radial errors (Figure ref:fig:ustation_errors_model_spindle_radial) and axial errors (Figure ref:fig:ustation_errors_model_spindle_axial).


%% Tomography experiment
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [0.9e-6; 0; 0]; % [m]

set_param(conf_simulink, 'StopTime', '10');

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('AP', P_micro_hexapod);

initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all');

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', false, ... % Translation Stage - X direction
    'Fdy_z', false, ... % Translation Stage - Z direction
    'Frz_x', true, ... % Spindle - X direction
    'Frz_y', true, ... % Spindle - Y direction
    'Frz_z', true);    % Spindle - Z direction

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1, ...
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

sim(mdl);
exp_tomography = simout;

%% Compare with the measurements
spindle_errors = load('ustation_errors_spindle.mat');

%% Measured radial errors of the Spindle
figure;
hold on;
plot(1e6*spindle_errors.Dx(1:100:end), 1e6*spindle_errors.Dy(1:100:end), 'DisplayName', 'Measurements')
plot(1e6*exp_tomography.y.x.Data, 1e6*exp_tomography.y.y.Data, 'DisplayName', 'Simulation')
hold off;
xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
axis equal
xlim([-1, 1]); ylim([-1, 1]);
xticks([-1, -0.5, 0, 0.5, 1]);
yticks([-1, -0.5, 0, 0.5, 1]);
leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Measured axial errors of the Spindle
figure;
hold on;
plot(spindle_errors.deg(1:100:end)/360, detrend(1e9*spindle_errors.Dz(1:100:end), 0), 'DisplayName', 'Measurements')
plot(exp_tomography.y.z.Time, detrend(1e9*exp_tomography.y.z.Data, 0), 'DisplayName', 'Simulation')
hold off;
xlabel('Rotation [turn]'); ylabel('Z displacement [nm]');
axis square
xlim([0,2]); ylim([-40, 40]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

% Raster Scans with the translation stage
% <<sec:ustation_experiments_ty_scans>>

% A second experiment was performed in which the translation stage was scanned at constant velocity.
% The translation stage setpoint is configured to have a "triangular" shape with stroke of $\pm 4.5\, mm$.
% Both ground motion and translation stage vibrations were included in the simulation.
% Similar to what was performed for the tomography simulation, the PoI position with respect to the granite was recorded and compared with the experimental measurements in Figure ref:fig:ustation_errors_model_dy_vertical.
% A similar error amplitude was observed, thus indicating that the multi-body model with the included disturbances accurately represented the micro-station behavior in typical scientific experiments.


%% Translation stage latteral scans
set_param(conf_simulink, 'StopTime', '2');

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();

initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all');

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', true, ... % Translation Stage - X direction
    'Fdy_z', true, ... % Translation Stage - Z direction
    'Frz_x', false, ... % Spindle - X direction
    'Frz_y', false, ... % Spindle - Y direction
    'Frz_z', false);    % Spindle - Z direction

initializeReferences(...
    'Dy_type', 'triangular', ...
    'Dy_amplitude', 4.5e-3, ...
    'Dy_period', 2);

sim(mdl);
exp_latteral_scans = simout;

% Load the experimentally measured errors
ty_errors = load('ustation_errors_ty.mat');

% Compute best straight line to remove it from data
average_error = mean(ty_errors.ty_z')';
straight_line = average_error - detrend(average_error, 1);

% Only plot data for the scan from -4.5mm to 4.5mm
dy_setpoint = 1e3*exp_latteral_scans.y.y.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5);
dz_error = detrend(1e6*exp_latteral_scans.y.z.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5), 0);

figure;
hold on;
plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,1) - straight_line, 0), 'color', colors(1,:), 'DisplayName', 'Measurement')
% plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,[4,6]) - straight_line, 0), 'color', colors(1,:), 'HandleVisibility', 'off')
plot(dy_setpoint, dz_error, 'color', colors(2,:), 'DisplayName', 'Simulation')
hold off;
xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
xlim([-5, 5]); ylim([-0.4, 0.4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;