function [inputs] = initializeInputs(opts_param) %% Default values for opts opts = struct( ... 'Dw', false, ... 'Dy', false, ... 'Ry', false, ... 'Rz', false, ... 'Dh', false, ... 'Rm', false, ... 'Dn', false ... ); %% Populate opts with input parameters if exist('opts_param','var') for opt = fieldnames(opts_param)' opts.(opt{1}) = opts_param.(opt{1}); end end %% Load Sampling Time and Simulation Time load('./mat/sim_conf.mat', 'sim_conf'); %% Define the time vector t = 0:sim_conf.Ts:sim_conf.Tsim; %% Ground motion - Dw if islogical(opts.Dw) && opts.Dw == true load('./mat/perturbations.mat', 'Wxg'); Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3); Dw(:, 1) = lsim(Wxg, Dw(:, 1), t); Dw(:, 2) = lsim(Wxg, Dw(:, 2), t); Dw(:, 3) = lsim(Wxg, Dw(:, 3), t); elseif islogical(opts.Dw) && opts.Dw == false Dw = zeros(length(t), 3); else Dw = opts.Dw; end %% Translation stage - Dy if islogical(opts.Dy) && opts.Dy == true Dy = zeros(length(t), 1); elseif islogical(opts.Dy) && opts.Dy == false Dy = zeros(length(t), 1); else Dy = opts.Dy; end %% Tilt Stage - Ry if islogical(opts.Ry) && opts.Ry == true Ry = 3*(2*pi/360)*sin(2*pi*0.2*t); elseif islogical(opts.Ry) && opts.Ry == false Ry = zeros(length(t), 1); else Ry = opts.Ry; end %% Spindle - Rz if islogical(opts.Rz) && opts.Rz == true Rz = 2*pi*0.5*t; elseif islogical(opts.Rz) && opts.Rz == false Rz = zeros(length(t), 1); elseif isnumeric(opts.Rz) && length(opts.Rz) == 1 Rz = 2*pi*(opts.Rz/60)*t; else Rz = opts.Rz; end %% Micro Hexapod - Dh if islogical(opts.Dh) && opts.Dh == true Dh = zeros(length(t), 6); elseif islogical(opts.Dh) && opts.Dh == false Dh = zeros(length(t), 6); else Dh = opts.Dh; end %% Axis Compensation - Rm if islogical(opts.Rm) Rm = zeros(length(t), 2); Rm(:, 2) = pi*ones(length(t), 1); else Rm = opts.Rm; end %% Nano Hexapod - Dn if islogical(opts.Dn) && opts.Dn == true Dn = zeros(length(t), 6); elseif islogical(opts.Dn) && opts.Dn == false Dn = zeros(length(t), 6); else Dn = opts.Dn; end %% Setpoint - Ds Ds = zeros(length(t), 6); for i = 1:length(t) Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i)); end %% External Forces applied on the Granite Fg = zeros(length(t), 3); %% External Forces applied on the Sample Fs = zeros(length(t), 3); %% Create the input Structure that will contain all the inputs inputs = struct( ... 't', t, ... 'Dw', Dw, ... 'Dy', Dy, ... 'Ry', Ry, ... 'Rz', Rz, ... 'Dh', Dh, ... 'Rm', Rm, ... 'Dn', Dn, ... 'Ds', Ds, ... 'Fg', Fg, ... 'Fs', Fs ... ); %% Save save('./mat/inputs.mat', 'inputs'); %% Custom Functions function setpoint = computeSetpoint(ty, ry, rz) %% setpoint = zeros(6, 1); %% Ty TMTy = [1 0 0 0 ; 0 1 0 ty ; 0 0 1 0 ; 0 0 0 1 ]; %% Ry TMRy = [ cos(ry) 0 sin(ry) 0 ; 0 1 0 0 ; -sin(ry) 0 cos(ry) 0 ; 0 0 0 1 ]; %% Rz TMRz = [cos(rz) -sin(rz) 0 0 ; sin(rz) cos(rz) 0 0 ; 0 0 1 0 ; 0 0 0 1 ]; %% All stages TM = TMTy*TMRy*TMRz; [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3)); setpoint(1:3) = TM(1:3, 4); setpoint(4:6) = [thetax, thetay, thetaz]; %% Custom Functions function [thetax, thetay, thetaz] = RM2angle(R) if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1 thetay = -asin(R(3, 1)); thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay)); thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay)); else thetaz = 0; if abs(R(3, 1)+1) < 1e-6 % R31 = -1 thetay = pi/2; thetax = thetaz + atan2(R(1, 2), R(1, 3)); else thetay = -pi/2; thetax = -thetaz + atan2(-R(1, 2), -R(1, 3)); end end end end end