tdehaeze/phd-simscape-micro-station
Archived
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working Simscape model

Browse Source
This commit is contained in:
Thomas Dehaeze
2024-10-30 14:29:52 +01:00
parent dfc36ad1ce
commit 45bef195d6
43 changed files with 918614 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
matlab/src/initializeDisturbances.m Normal file
View File

@@ -0,0 +1,148 @@
function [] = initializeDisturbances(args)
% initializeDisturbances - Initialize the disturbances
%
% Syntax: [] = initializeDisturbances(args)
%
% Inputs:
% - args -
arguments
% Global parameter to enable or disable the disturbances
args.enable logical {mustBeNumericOrLogical} = true
% Ground Motion - X direction
args.Dwx logical {mustBeNumericOrLogical} = true
% Ground Motion - Y direction
args.Dwy logical {mustBeNumericOrLogical} = true
% Ground Motion - Z direction
args.Dwz logical {mustBeNumericOrLogical} = true
% Translation Stage - X direction
args.Fty_x logical {mustBeNumericOrLogical} = true
% Translation Stage - Z direction
args.Fty_z logical {mustBeNumericOrLogical} = true
% Spindle - Z direction
args.Frz_z logical {mustBeNumericOrLogical} = true
end
load('./mat/dist_psd.mat', 'dist_f');
dist_f.f = dist_f.f(2:end);
dist_f.psd_gm = dist_f.psd_gm(2:end);
dist_f.psd_ty = dist_f.psd_ty(2:end);
dist_f.psd_rz = dist_f.psd_rz(2:end);
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
T0 = N/Fs; % Signal Duration [s]
df = 1/T0; % Frequency resolution of the DFT [Hz]
% Also equal to (dist_f.f(2)-dist_f.f(1))
t = linspace(0, T0, N+1)'; % Time Vector [s]
Ts = 1/Fs; % Sampling Time [s]
phi = dist_f.psd_gm;
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
if args.Dwx && args.enable
rng(111);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
else
Dwx = zeros(length(t), 1);
end
if args.Dwy && args.enable
rng(112);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
else
Dwy = zeros(length(t), 1);
end
if args.Dwy && args.enable
rng(113);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
else
Dwz = zeros(length(t), 1);
end
if args.Fty_x && args.enable
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
rng(121);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
Fty_x = u;
else
Fty_x = zeros(length(t), 1);
end
if args.Fty_z && args.enable
phi = dist_f.psd_ty;
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
rng(122);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
Fty_z = u;
else
Fty_z = zeros(length(t), 1);
end
if args.Frz_z && args.enable
phi = dist_f.psd_rz;
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
rng(131);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
Frz_z = u;
else
Frz_z = zeros(length(t), 1);
end
u = zeros(length(t), 6);
Fd = u;
Dwx = Dwx - Dwx(1);
Dwy = Dwy - Dwy(1);
Dwz = Dwz - Dwz(1);
Fty_x = Fty_x - Fty_x(1);
Fty_z = Fty_z - Fty_z(1);
Frz_z = Frz_z - Frz_z(1);
micro_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nass_disturbances.mat', 'file')
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args', '-append');
else
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_disturbances.mat', 'file')
save('matlab/mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args', '-append');
else
save('matlab/mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
end
end

57
matlab/src/initializeGranite.m Normal file
View File

@@ -0,0 +1,57 @@
function [granite] = initializeGranite(args)
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
args.sample_pos (1,1) double {mustBeNumeric} = 0.8 % Height of the measurment point [m]
end
granite = struct();
switch args.type
case 'none'
granite.type = 0;
case 'rigid'
granite.type = 1;
case 'flexible'
granite.type = 2;
case 'modal-analysis'
granite.type = 3;
case 'init'
granite.type = 4;
end
granite.density = args.density; % [kg/m3]
granite.STEP = 'granite.STEP';
granite.sample_pos = args.sample_pos; % [m]
granite.K = args.K; % [N/m]
granite.C = args.C; % [N/(m/s)]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fgm');
granite.Deq = -Fgm'./granite.K;
else
granite.Deq = zeros(6,1);
end
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'granite', '-append');
else
save('mat/nass_stages.mat', 'granite');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'granite', '-append');
else
save('matlab/mat/nass_stages.mat', 'granite');
end
end

34
matlab/src/initializeGround.m Normal file
View File

@@ -0,0 +1,34 @@
function [ground] = initializeGround(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
end
ground = struct();
switch args.type
case 'none'
ground.type = 0;
case 'rigid'
ground.type = 1;
end
ground.shape = [2, 2, 0.5]; % [m]
ground.density = 2800; % [kg/m3]
ground.rot_point = args.rot_point;
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'ground', '-append');
else
save('mat/nass_stages.mat', 'ground');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'ground', '-append');
else
save('matlab/mat/nass_stages.mat', 'ground');
end
end

33
matlab/src/initializeLoggingConfiguration.m Normal file
View File

@@ -0,0 +1,33 @@
function [] = initializeLoggingConfiguration(args)
arguments
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
end
conf_log = struct();
switch args.log
case 'none'
conf_log.type = 0;
case 'all'
conf_log.type = 1;
case 'forces'
conf_log.type = 2;
end
conf_log.Ts = args.Ts;
if exist('./mat', 'dir')
if exist('./mat/conf_log.mat', 'file')
save('mat/conf_log.mat', 'conf_log', '-append');
else
save('mat/conf_log.mat', 'conf_log');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/conf_log.mat', 'file')
save('matlab/mat/conf_log.mat', 'conf_log', '-append');
else
save('matlab/mat/conf_log.mat', 'conf_log');
end
end

122
matlab/src/initializeMicroHexapod.m Normal file
View File

@@ -0,0 +1,122 @@
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
% generateGeneralConfiguration
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
% initializeStrutDynamics
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
% initializeCylindricalPlatforms
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
% initializeCylindricalStruts
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
% Force that stiffness of each joint should apply at t=0
args.Foffset logical {mustBeNumericOrLogical} = false
end
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, ...
'H', args.H, ...
'MO_B', args.MO_B);
stewart = generateGeneralConfiguration(stewart, ...
'FH', args.FH, ...
'FR', args.FR, ...
'FTh', args.FTh, ...
'MH', args.MH, ...
'MR', args.MR, ...
'MTh', args.MTh);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, ...
'K', args.Ki, ...
'C', args.Ci);
stewart = initializeJointDynamics(stewart, ...
'type_F', 'universal_p', ...
'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart, ...
'Fpm', args.Fpm, ...
'Fph', args.Fph, ...
'Fpr', args.Fpr, ...
'Mpm', args.Mpm, ...
'Mph', args.Mph, ...
'Mpr', args.Mpr);
stewart = initializeCylindricalStruts(stewart, ...
'Fsm', args.Fsm, ...
'Fsh', args.Fsh, ...
'Fsr', args.Fsr, ...
'Msm', args.Msm, ...
'Msh', args.Msh, ...
'Msr', args.Msr);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart, ...
'AP', args.AP, ...
'ARB', args.ARB);
stewart = initializeInertialSensor(stewart, 'type', 'none');
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fhm');
stewart.actuators.dLeq = -Fhm'./args.Ki;
else
stewart.actuators.dLeq = zeros(6,1);
end
switch args.type
case 'none'
stewart.type = 0;
case 'rigid'
stewart.type = 1;
case 'flexible'
stewart.type = 2;
case 'modal-analysis'
stewart.type = 3;
case 'init'
stewart.type = 4;
case 'compliance'
stewart.type = 5;
end
micro_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'micro_hexapod', '-append');
else
save('mat/nass_stages.mat', 'micro_hexapod');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'micro_hexapod', '-append');
else
save('matlab/mat/nass_stages.mat', 'micro_hexapod');
end
end

241
matlab/src/initializeReferences.m Normal file
View File

@@ -0,0 +1,241 @@
function [ref] = initializeReferences(args)
arguments
% Sampling Frequency [s]
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
% Maximum simulation time [s]
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
% Either "constant" / "triangular" / "sinusoidal"
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude of the displacement [m]
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "triangular" / "sinusoidal"
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude [rad]
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "rotating"
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
% Initial angle [rad]
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the rotating [s]
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% For now, only constant is implemented
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
% For now, only constant is implemented
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
% Initial position of the two masses
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
% For now, only constant is implemented
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
end
%% Set Sampling Time
Ts = args.Ts;
Tmax = args.Tmax;
%% Low Pass Filter to filter out the references
s = zpk('s');
w0 = 2*pi*10;
xi = 1;
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
%% Translation stage - Dy
t = 0:Ts:Tmax; % Time Vector [s]
Dy = zeros(length(t), 1);
Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1);
switch args.Dy_type
case 'constant'
Dy(:) = args.Dy_amplitude;
Dyd(:) = 0;
Dydd(:) = 0;
case 'triangular'
% This is done to unsure that we start with no displacement
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
i0 = find(t>=args.Dy_period/4,1);
Dy(1:end-i0+1) = Dy_raw(i0:end);
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
% The signal is filtered out
Dy = lsim(H_lpf, Dy, t);
Dyd = lsim(H_lpf*s, Dy, t);
Dydd = lsim(H_lpf*s^2, Dy, t);
case 'sinusoidal'
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
otherwise
warning('Dy_type is not set correctly');
end
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
%% Tilt Stage - Ry
t = 0:Ts:Tmax; % Time Vector [s]
Ry = zeros(length(t), 1);
Ryd = zeros(length(t), 1);
Rydd = zeros(length(t), 1);
switch args.Ry_type
case 'constant'
Ry(:) = args.Ry_amplitude;
Ryd(:) = 0;
Rydd(:) = 0;
case 'triangular'
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
i0 = find(t>=args.Ry_period/4,1);
Ry(1:end-i0+1) = Ry_raw(i0:end);
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
% The signal is filtered out
Ry = lsim(H_lpf, Ry, t);
Ryd = lsim(H_lpf*s, Ry, t);
Rydd = lsim(H_lpf*s^2, Ry, t);
case 'sinusoidal'
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
otherwise
warning('Ry_type is not set correctly');
end
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
%% Spindle - Rz
t = 0:Ts:Tmax; % Time Vector [s]
Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1);
switch args.Rz_type
case 'constant'
Rz(:) = args.Rz_amplitude;
Rzd(:) = 0;
Rzdd(:) = 0;
case 'rotating-not-filtered'
Rz(:) = 2*pi/args.Rz_period*t;
% The signal is filtered out
Rz(:) = 2*pi/args.Rz_period*t;
Rzd(:) = 2*pi/args.Rz_period;
Rzdd(:) = 0;
% We add the angle offset
Rz = Rz + args.Rz_amplitude;
case 'rotating'
Rz(:) = 2*pi/args.Rz_period*t;
% The signal is filtered out
Rz = lsim(H_lpf, Rz, t);
Rzd = lsim(H_lpf*s, Rz, t);
Rzdd = lsim(H_lpf*s^2, Rz, t);
% We add the angle offset
Rz = Rz + args.Rz_amplitude;
otherwise
warning('Rz_type is not set correctly');
end
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
%% Micro-Hexapod
t = [0, Ts];
Dh = zeros(length(t), 6);
Dhl = zeros(length(t), 6);
switch args.Dh_type
case 'constant'
Dh = [args.Dh_pos, args.Dh_pos];
load('nass_stages.mat', 'micro_hexapod');
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
tx = args.Dh_pos(4);
ty = args.Dh_pos(5);
tz = args.Dh_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
Dhl = [Dhl, Dhl];
otherwise
warning('Dh_type is not set correctly');
end
Dh = struct('time', t, 'signals', struct('values', Dh));
Dhl = struct('time', t, 'signals', struct('values', Dhl));
%% Axis Compensation - Rm
t = [0, Ts];
Rm = [args.Rm_pos, args.Rm_pos];
Rm = struct('time', t, 'signals', struct('values', Rm));
%% Nano-Hexapod
t = [0, Ts];
Dn = zeros(length(t), 6);
switch args.Dn_type
case 'constant'
Dn = [args.Dn_pos, args.Dn_pos];
load('nass_stages.mat', 'nano_hexapod');
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
tx = args.Dn_pos(4);
ty = args.Dn_pos(5);
tz = args.Dn_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
Dnl = [Dnl, Dnl];
otherwise
warning('Dn_type is not set correctly');
end
Dn = struct('time', t, 'signals', struct('values', Dn));
Dnl = struct('time', t, 'signals', struct('values', Dnl));
micro_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nass_references.mat', 'file')
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
else
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_references.mat', 'file')
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
else
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
end
end

66
matlab/src/initializeRy.m Normal file
View File

@@ -0,0 +1,66 @@
function [ry] = initializeRy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.Ry_init (1,1) double {mustBeNumeric} = 0
end
ry = struct();
switch args.type
case 'none'
ry.type = 0;
case 'rigid'
ry.type = 1;
case 'flexible'
ry.type = 2;
case 'modal-analysis'
ry.type = 3;
case 'init'
ry.type = 4;
end
% Ry - Guide for the tilt stage
ry.guide.density = 7800; % [kg/m3]
ry.guide.STEP = 'Tilt_Guide.STEP';
% Ry - Rotor of the motor
ry.rotor.density = 2400; % [kg/m3]
ry.rotor.STEP = 'Tilt_Motor_Axis.STEP';
% Ry - Motor
ry.motor.density = 3200; % [kg/m3]
ry.motor.STEP = 'Tilt_Motor.STEP';
% Ry - Plateau Tilt
ry.stage.density = 7800; % [kg/m3]
ry.stage.STEP = 'Tilt_Stage.STEP';
ry.z_offset = 0.58178; % [m]
ry.Ry_init = args.Ry_init; % [rad]
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fym');
ry.Deq = -Fym'./ry.K;
else
ry.Deq = zeros(6,1);
end
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'ry', '-append');
else
save('mat/nass_stages.mat', 'ry');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'ry', '-append');
else
save('matlab/mat/nass_stages.mat', 'ry');
end
end

57
matlab/src/initializeRz.m Normal file
View File

@@ -0,0 +1,57 @@
function [rz] = initializeRz(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
end
rz = struct();
switch args.type
case 'none'
rz.type = 0;
case 'rigid'
rz.type = 1;
case 'flexible'
rz.type = 2;
case 'modal-analysis'
rz.type = 3;
case 'init'
rz.type = 4;
end
% Spindle - Slip Ring
rz.slipring.density = 7800; % [kg/m3]
rz.slipring.STEP = 'Spindle_Slip_Ring.STEP';
% Spindle - Rotor
rz.rotor.density = 7800; % [kg/m3]
rz.rotor.STEP = 'Spindle_Rotor.STEP';
% Spindle - Stator
rz.stator.density = 7800; % [kg/m3]
rz.stator.STEP = 'Spindle_Stator.STEP';
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fzm');
rz.Deq = -Fzm'./rz.K;
else
rz.Deq = zeros(6,1);
end
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'rz', '-append');
else
save('mat/nass_stages.mat', 'rz');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'rz', '-append');
else
save('matlab/mat/nass_stages.mat', 'rz');
end
end

27
matlab/src/initializeSimscapeConfiguration.m Normal file
View File

@@ -0,0 +1,27 @@
function [] = initializeSimscapeConfiguration(args)
arguments
args.gravity logical {mustBeNumericOrLogical} = true
end
conf_simscape = struct();
if args.gravity
conf_simscape.type = 1;
else
conf_simscape.type = 2;
end
if exist('./mat', 'dir')
if exist('./mat/conf_simscape.mat', 'file')
save('mat/conf_simscape.mat', 'conf_simscape', '-append');
else
save('mat/conf_simscape.mat', 'conf_simscape');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/conf_simscape.mat', 'file')
save('matlab/mat/conf_simscape.mat', 'conf_simscape', '-append');
else
save('matlab/mat/conf_simscape.mat', 'conf_simscape');
end
end

81
matlab/src/initializeTy.m Normal file
View File

@@ -0,0 +1,81 @@
function [ty] = initializeTy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
end
ty = struct();
switch args.type
case 'none'
ty.type = 0;
case 'rigid'
ty.type = 1;
case 'flexible'
ty.type = 2;
case 'modal-analysis'
ty.type = 3;
case 'init'
ty.type = 4;
end
% Ty Granite frame
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
ty.granite_frame.STEP = 'Ty_Granite_Frame.STEP';
% Guide Translation Ty
ty.guide.density = 7800; % [kg/m3] => 76kg
ty.guide.STEP = 'Ty_Guide.STEP';
% Ty - Guide_Translation12
ty.guide12.density = 7800; % [kg/m3]
ty.guide12.STEP = 'Ty_Guide_12.STEP';
% Ty - Guide_Translation11
ty.guide11.density = 7800; % [kg/m3]
ty.guide11.STEP = 'Ty_Guide_11.STEP';
% Ty - Guide_Translation22
ty.guide22.density = 7800; % [kg/m3]
ty.guide22.STEP = 'Ty_Guide_22.STEP';
% Ty - Guide_Translation21
ty.guide21.density = 7800; % [kg/m3]
ty.guide21.STEP = 'Ty_Guide_21.STEP';
% Ty - Plateau translation
ty.frame.density = 7800; % [kg/m3]
ty.frame.STEP = 'Ty_Stage.STEP';
% Ty Stator Part
ty.stator.density = 5400; % [kg/m3]
ty.stator.STEP = 'Ty_Motor_Stator.STEP';
% Ty Rotor Part
ty.rotor.density = 5400; % [kg/m3]
ty.rotor.STEP = 'Ty_Motor_Rotor.STEP';
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Ftym');
ty.Deq = -Ftym'./ty.K;
else
ty.Deq = zeros(6,1);
end
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'ty', '-append');
else
save('mat/nass_stages.mat', 'ty');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'ty', '-append');
else
save('matlab/mat/nass_stages.mat', 'ty');
end
end

31
matlab/src/initializeZAxisAccelerometer.m Normal file
View File

@@ -0,0 +1,31 @@
function [accelerometer] = initializeZAxisAccelerometer(args)
arguments
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz]
end
%%
accelerometer.m = args.mass;
%% The Stiffness is set to have the damping resonance frequency
accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;
%% We set the damping value to have critical damping
accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);
%% Gain correction of the accelerometer to have a unity gain until the resonance
accelerometer.gain = -accelerometer.k/accelerometer.m;
if exist('./mat', 'dir')
if exist('./mat/accelerometer_z_axis.mat', 'file')
save('mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
else
save('mat/accelerometer_z_axis.mat', 'accelerometer');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/accelerometer_z_axis.mat', 'file')
save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
else
save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer');
end
end

28
matlab/src/initializeZAxisGeophone.m Normal file
View File

@@ -0,0 +1,28 @@
function [geophone] = initializeZAxisGeophone(args)
arguments
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz]
end
%%
geophone.m = args.mass;
%% The Stiffness is set to have the damping resonance frequency
geophone.k = geophone.m * (2*pi*args.freq)^2;
%% We set the damping value to have critical damping
geophone.c = 2*sqrt(geophone.m * geophone.k);
if exist('./mat', 'dir')
if exist('./mat/geophone_z_axis.mat', 'file')
save('mat/geophone_z_axis.mat', 'geophone', '-append');
else
save('mat/geophone_z_axis.mat', 'geophone');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/geophone_z_axis.mat', 'file')
save('matlab/mat/geophone_z_axis.mat', 'geophone', '-append');
else
save('matlab/mat/geophone_z_axis.mat', 'geophone');
end
end