104 lines
4.8 KiB
Mathematica
104 lines
4.8 KiB
Mathematica
|
%%
|
|||
|
|
clear; close all; clc;
|
|||
|
|
|
|||
|
|
%% Initialize simulation configuration
|
|||
|
|
opts_sim = struct(...
|
|||
|
|
'Tsim', 30 ...
|
|||
|
|
);
|
|||
|
|
|
|||
|
|
initializeSimConf(opts_sim);
|
|||
|
|
|
|||
|
|
%% Initialize Inputs
|
|||
|
|
load('./mat/sim_conf.mat', 'sim_conf')
|
|||
|
|
|
|||
|
|
time_vector = 0:sim_conf.Ts:sim_conf.Tsim;
|
|||
|
|
|
|||
|
|
% Translation Stage
|
|||
|
|
T_ty = 4;
|
|||
|
|
ty = zeros(length(time_vector), 1);
|
|||
|
|
ty(1:T_ty/sim_conf.Ts) = 10e-3*sin(2*pi*(1/2)*time_vector(1:T_ty/sim_conf.Ts));
|
|||
|
|
|
|||
|
|
% Tilt Stage
|
|||
|
|
T_ry = 4;
|
|||
|
|
ry = zeros(length(time_vector), 1);
|
|||
|
|
ry((T_ty)/sim_conf.Ts:(T_ty+T_ry)/sim_conf.Ts) = 2*pi*(3/360)*sin(2*pi*(1/2)*time_vector(T_ty/sim_conf.Ts:(T_ty+T_ry)/sim_conf.Ts));
|
|||
|
|
|
|||
|
|
% Spindle
|
|||
|
|
T_rz = 4;
|
|||
|
|
|
|||
|
|
rz = zeros(length(time_vector), 1);
|
|||
|
|
rz((T_ty+T_ry)/sim_conf.Ts:(T_ty+T_ry+T_rz)/sim_conf.Ts) = 2*pi*0.5*(time_vector((T_ty+T_ry)/sim_conf.Ts:(T_ty+T_ry+T_rz)/sim_conf.Ts)-time_vector((T_ty+T_ry)/sim_conf.Ts));
|
|||
|
|
rz((T_ty+T_ry+T_rz)/sim_conf.Ts:end) = rz((T_ty+T_ry+T_rz)/sim_conf.Ts);
|
|||
|
|
|
|||
|
|
% Micro Hexapod
|
|||
|
|
T_u_hexa = 10;
|
|||
|
|
u_hexa = zeros(length(time_vector), 6);
|
|||
|
|
% Tz
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz)/sim_conf.Ts:(T_ty+T_ry+T_rz+2)/sim_conf.Ts, 3) = 10e-3*sin(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz)/sim_conf.Ts:(T_ty+T_ry+T_rz+2)/sim_conf.Ts)));
|
|||
|
|
% Tx-Ty
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+2)/sim_conf.Ts:(T_ty+T_ry+T_rz+3)/sim_conf.Ts, 1) = 10e-3*(time_vector((T_ty+T_ry+T_rz+2)/sim_conf.Ts:(T_ty+T_ry+T_rz+3)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+2)/sim_conf.Ts));
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts, 1) = 10e-3*cos(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts)));
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts, 2) = 10e-3*sin(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts)));
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+5)/sim_conf.Ts:(T_ty+T_ry+T_rz+6)/sim_conf.Ts, 1) = 10e-3 - 10e-3*(time_vector((T_ty+T_ry+T_rz+5)/sim_conf.Ts:(T_ty+T_ry+T_rz+6)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+5)/sim_conf.Ts));
|
|||
|
|
% Theta x Theta y
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+6)/sim_conf.Ts:(T_ty+T_ry+T_rz+7)/sim_conf.Ts, 1) = 2*pi*(3/360)*(time_vector((T_ty+T_ry+T_rz+6)/sim_conf.Ts:(T_ty+T_ry+T_rz+7)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+6)/sim_conf.Ts));
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts, 1) = 2*pi*(3/360)*cos(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts)));
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts, 2) = 2*pi*(3/360)*sin(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts)));
|
|||
|
|
u_hexa((T_ty+T_ry+T_rz+9)/sim_conf.Ts:(T_ty+T_ry+T_rz+10)/sim_conf.Ts, 1) = 2*pi*(3/360) - 2*pi*(3/360)*(time_vector((T_ty+T_ry+T_rz+9)/sim_conf.Ts:(T_ty+T_ry+T_rz+10)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+9)/sim_conf.Ts));
|
|||
|
|
|
|||
|
|
% Gravity Compensator system
|
|||
|
|
T_mass_start = T_ty+T_ry+T_rz+T_u_hexa;
|
|||
|
|
mass = zeros(length(time_vector), 2);
|
|||
|
|
|
|||
|
|
mass((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts, 1) = 2*pi*( 20/360)*(time_vector((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts)-time_vector(T_mass_start/sim_conf.Ts));
|
|||
|
|
mass((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts, 2) = 2*pi*(-10/360)*(time_vector((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts)-time_vector(T_mass_start/sim_conf.Ts));
|
|||
|
|
mass((T_mass_start+2)/sim_conf.Ts:(T_mass_start+3)/sim_conf.Ts, 1) = mass((T_mass_start+2)/sim_conf.Ts, 1);
|
|||
|
|
mass((T_mass_start+2)/sim_conf.Ts:(T_mass_start+3)/sim_conf.Ts, 2) = mass((T_mass_start+2)/sim_conf.Ts, 2);
|
|||
|
|
mass((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts, 1) = mass((T_mass_start+2)/sim_conf.Ts, 1)-2*pi*( 20/360)*(time_vector((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts)-time_vector((T_mass_start+3)/sim_conf.Ts));
|
|||
|
|
mass((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts, 2) = mass((T_mass_start+2)/sim_conf.Ts, 2)-2*pi*(-10/360)*(time_vector((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts)-time_vector((T_mass_start+3)/sim_conf.Ts));
|
|||
|
|
|
|||
|
|
% opts_inputs = struct(...
|
|||
|
|
% 'ty', ty, ...
|
|||
|
|
% 'ry', ry, ...
|
|||
|
|
% 'rz', rz, ...
|
|||
|
|
% 'u_hexa', u_hexa, ...
|
|||
|
|
% 'mass', mass ...
|
|||
|
|
% );
|
|||
|
|
|
|||
|
|
% initializeInputs(opts_inputs);
|
|||
|
|
initializeInputs();
|
|||
|
|
|
|||
|
|
%% Initialize SolidWorks Data
|
|||
|
|
initializeSmiData();
|
|||
|
|
|
|||
|
|
%% Initialize Ground
|
|||
|
|
initializeGround();
|
|||
|
|
|
|||
|
|
%% Initialize Granite
|
|||
|
|
initializeGranite();
|
|||
|
|
|
|||
|
|
%% Initialize Translation stage
|
|||
|
|
initializeTy();
|
|||
|
|
|
|||
|
|
%% Initialize Tilt Stage
|
|||
|
|
initializeRy();
|
|||
|
|
|
|||
|
|
%% Initialize Spindle
|
|||
|
|
initializeRz();
|
|||
|
|
|
|||
|
|
%% Initialize Hexapod Sym<EFBFBD>trie
|
|||
|
|
initializeMicroHexapod();
|
|||
|
|
|
|||
|
|
%% Initialize Center of Gravity compensation
|
|||
|
|
initializeAxisc();
|
|||
|
|
|
|||
|
|
%% Initialize NASS
|
|||
|
|
opts_nano_hexapod = struct('actuator', 'lorentz');
|
|||
|
|
|
|||
|
|
initializeNanoHexapod(opts_nano_hexapod);
|
|||
|
|
|
|||
|
|
%% Initialize Sample
|
|||
|
|
opts_sample = struct('mass', 20);
|
|||
|
|
|
|||
|
|
initializeSample(opts_sample);
|