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

Re-tuned micro-station model

Browse Source
This commit is contained in:
Thomas Dehaeze
2024-10-31 10:37:01 +01:00
parent 151ae07072
commit 04c8b3c9dc
12 changed files with 563 additions and 554 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
matlab/src/computeReferencePose.m Normal file
View File

@@ -0,0 +1,77 @@
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
%
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
%
% Inputs:
% - Dy - Reference of the Translation Stage [m]
% - Ry - Reference of the Tilt Stage [rad]
% - Rz - Reference of the Spindle [rad]
% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
%
% Outputs:
% - WTr -
%% Translation Stage
Rty = [1 0 0 0;
0 1 0 Dy;
0 0 1 0;
0 0 0 1];
%% Tilt Stage - Pure rotating aligned with Ob
Rry = [ cos(Ry) 0 sin(Ry) 0;
0 1 0 0;
-sin(Ry) 0 cos(Ry) 0;
0 0 0 1];
%% Spindle - Rotation along the Z axis
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
sin(Rz) cos(Rz) 0 0 ;
0 0 1 0 ;
0 0 0 1 ];
%% Micro-Hexapod
Rhx = [1 0 0;
0 cos(Dh(4)) -sin(Dh(4));
0 sin(Dh(4)) cos(Dh(4))];
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
0 1 0;
-sin(Dh(5)) 0 cos(Dh(5))];
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
sin(Dh(6)) cos(Dh(6)) 0;
0 0 1];
Rh = [1 0 0 Dh(1) ;
0 1 0 Dh(2) ;
0 0 1 Dh(3) ;
0 0 0 1 ];
Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
%% Nano-Hexapod
Rnx = [1 0 0;
0 cos(Dn(4)) -sin(Dn(4));
0 sin(Dn(4)) cos(Dn(4))];
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
0 1 0;
-sin(Dn(5)) 0 cos(Dn(5))];
Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
sin(Dn(6)) cos(Dn(6)) 0;
0 0 1];
Rn = [1 0 0 Dn(1) ;
0 1 0 Dn(2) ;
0 0 1 Dn(3) ;
0 0 0 1 ];
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
%% Total Homogeneous transformation
WTr = Rty*Rry*Rrz*Rh*Rn;
end

147
matlab/src/describeMicroStationSetup.m Normal file
View File

@@ -0,0 +1,147 @@
function [] = describeMicroStationSetup()
% describeMicroStationSetup -
%
% Syntax: [] = describeMicroStationSetup()
%
% Inputs:
% - -
%
% Outputs:
% - -
load('./mat/conf_simscape.mat', 'conf_simscape');
fprintf('Simscape Configuration:\n');
if conf_simscape.type == 1
fprintf('- Gravity is included\n');
else
fprintf('- Gravity is not included\n');
end
fprintf('\n');
load('./mat/nass_disturbances.mat', 'args');
fprintf('Disturbances:\n');
if ~args.enable
fprintf('- No disturbance is included\n');
else
if args.Dwx && args.Dwy && args.Dwz
fprintf('- Ground motion\n');
end
if args.Fty_x && args.Fty_z
fprintf('- Vibrations of the Translation Stage\n');
end
if args.Frz_z
fprintf('- Vibrations of the Spindle\n');
end
end
fprintf('\n');
load('./mat/nass_references.mat', 'args');
fprintf('Reference Tracking:\n');
fprintf('- Translation Stage:\n');
switch args.Dy_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
case 'triangular'
fprintf(' - Triangular Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
case 'sinusoidal'
fprintf(' - Sinusoidal Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
end
fprintf('- Tilt Stage:\n');
switch args.Ry_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
case 'triangular'
fprintf(' - Triangular Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
case 'sinusoidal'
fprintf(' - Sinusoidal Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
end
fprintf('- Spindle:\n');
switch args.Rz_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
case { 'rotating', 'rotating-not-filtered' }
fprintf(' - Rotating\n');
fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period);
end
fprintf('- Micro Hexapod:\n');
switch args.Dh_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
end
fprintf('\n');
load('./mat/controller.mat', 'controller');
fprintf('Controller:\n');
fprintf('- %s\n', controller.name);
fprintf('\n');
load('./mat/stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
fprintf('Micro Station:\n');
if granite.type == 1 && ...
ty.type == 1 && ...
ry.type == 1 && ...
rz.type == 1 && ...
micro_hexapod.type == 1;
fprintf('- All stages are rigid\n');
elseif granite.type == 2 && ...
ty.type == 2 && ...
ry.type == 2 && ...
rz.type == 2 && ...
micro_hexapod.type == 2;
fprintf('- All stages are flexible\n');
else
if granite.type == 1 || granite.type == 4
fprintf('- Granite is rigid\n');
else
fprintf('- Granite is flexible\n');
end
if ty.type == 1 || ty.type == 4
fprintf('- Translation Stage is rigid\n');
else
fprintf('- Translation Stage is flexible\n');
end
if ry.type == 1 || ry.type == 4
fprintf('- Tilt Stage is rigid\n');
else
fprintf('- Tilt Stage is flexible\n');
end
if rz.type == 1 || rz.type == 4
fprintf('- Spindle is rigid\n');
else
fprintf('- Spindle is flexible\n');
end
if micro_hexapod.type == 1 || micro_hexapod.type == 4
fprintf('- Micro Hexapod is rigid\n');
else
fprintf('- Micro Hexapod is flexible\n');
end
end
fprintf('\n');

18
matlab/src/initializeGranite.m
View File

@@ -1,11 +1,10 @@
function [granite] = initializeGranite(args)
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
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.K (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9; 5e9; 5e9; 2.5e7; 2.5e7; 1e7] % [N/m]
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4; 2e4; 1e4] % [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]
@@ -21,10 +20,6 @@
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]
@@ -35,13 +30,6 @@
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');

17
matlab/src/initializeMicroHexapod.m
View File

@@ -1,7 +1,7 @@
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
@@ -32,8 +32,6 @@
% 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();
@@ -84,13 +82,6 @@
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;
@@ -98,12 +89,6 @@
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;

14
matlab/src/initializeRy.m
View File

@@ -1,8 +1,7 @@
function [ry] = initializeRy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
args.Ry_init (1,1) double {mustBeNumeric} = 0
end
@@ -15,10 +14,6 @@
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
@@ -44,13 +39,6 @@
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');

14
matlab/src/initializeRz.m
View File

@@ -1,8 +1,7 @@
function [rz] = initializeRz(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
end
rz = struct();
@@ -14,10 +13,6 @@
rz.type = 1;
case 'flexible'
rz.type = 2;
case 'modal-analysis'
rz.type = 3;
case 'init'
rz.type = 4;
end
% Spindle - Slip Ring
@@ -35,13 +30,6 @@
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');

16
matlab/src/initializeTy.m
View File

@@ -1,8 +1,7 @@
function [ty] = initializeTy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
end
ty = struct();
@@ -14,10 +13,6 @@
ty.type = 1;
case 'flexible'
ty.type = 2;
case 'modal-analysis'
ty.type = 3;
case 'init'
ty.type = 4;
end
% Ty Granite frame
@@ -57,14 +52,7 @@
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
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')

BIN
matlab/subsystems/micro_hexapod_bot_plate.slx
View File

Binary file not shown.

BIN
matlab/subsystems/micro_hexapod_strut.slx
View File

Binary file not shown.

BIN
matlab/subsystems/micro_hexapod_top_plate.slx
View File

Binary file not shown.

BIN
matlab/ustation_simscape.slx
View File

Binary file not shown.