173 lines
4.5 KiB
Matlab
173 lines
4.5 KiB
Matlab
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( ...
|
|
'Dw', timeseries(Dw, t), ...
|
|
'Dy', timeseries(Dy, t), ...
|
|
'Ry', timeseries(Ry, t), ...
|
|
'Rz', timeseries(Rz, t), ...
|
|
'Dh', timeseries(Dh, t), ...
|
|
'Rm', timeseries(Rm, t), ...
|
|
'Dn', timeseries(Dn, t), ...
|
|
'Ds', timeseries(Ds, t), ...
|
|
'Fg', timeseries(Fg, t), ...
|
|
'Fs', timeseries(Fs, t) ...
|
|
);
|
|
|
|
%% 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
|