add all files

2025-04-14 18:38:19 +02:00
parent 3cc2105324
commit 37cd117a8f
859 changed files with 5418446 additions and 0 deletions
--- a/A4-simscape-micro-station/ustation_1_kinematics.m
+++ b/A4-simscape-micro-station/ustation_1_kinematics.m
@@ -0,0 +1,90 @@
+%% ustation_1_kinematics.m
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for Data
+addpath('./src/'); % Path for functions
+addpath('./STEPS/'); % Path for STEPS
+addpath('./subsystems/'); % Path for Subsystems Simulink files
+
+% Simulink Model name
+mdl = 'ustation_simscape';
+
+load('nass_model_conf_simulink.mat');
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(log10(10), log10(2e3), 1000);
+
+%% Stage setpoints
+Dy = 1e-3; % Translation Stage [m]
+Ry = 3*pi/180; % Tilt Stage [rad]
+Rz = 180*pi/180; % Spindle [rad]
+
+%% Stage individual Homogeneous transformations
+% 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-Station homogeneous transformation
+Ttot = Rty*Rry*Rrz;
+
+%% Compute translations and rotations (Euler angles) induced by the micro-station
+ustation_dx = Ttot(1,4);
+ustation_dy = Ttot(2,4);
+ustation_dz = Ttot(3,4);
+ustation_ry = atan2( Ttot(1, 3),                  sqrt(Ttot(1, 1)^2 + Ttot(1, 2)^2));
+ustation_rx = atan2(-Ttot(2, 3)/cos(ustation_ry), Ttot(3, 3)/cos(ustation_ry));
+ustation_rz = atan2(-Ttot(1, 2)/cos(ustation_ry), Ttot(1, 1)/cos(ustation_ry));
+
+%% Verification using the Simscape model
+% All stages are initialized as rigid bodies to avoid any guiding error
+initializeGround(      'type', 'rigid');
+initializeGranite(     'type', 'rigid');
+initializeTy(          'type', 'rigid');
+initializeRy(          'type', 'rigid');
+initializeRz(          'type', 'rigid');
+initializeMicroHexapod('type', 'rigid');
+
+initializeLoggingConfiguration('log', 'all');
+
+initializeReferences('Dy_amplitude', Dy, ...
+                     'Ry_amplitude', Ry, ...
+                     'Rz_amplitude', Rz);
+
+initializeDisturbances('enable', false);
+
+set_param(conf_simulink, 'StopTime', '0.5');
+
+% Simulation is performed
+sim(mdl);
+
+% Sample's motion is computed from "external metrology"
+T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end); simout.y.z.Data(end)]; [0,0,0,1]];
+sim_dx = T_sim(1,4);
+sim_dy = T_sim(2,4);
+sim_dz = T_sim(3,4);
+sim_ry = atan2( T_sim(1, 3),             sqrt(T_sim(1, 1)^2 + T_sim(1, 2)^2));
+sim_rx = atan2(-T_sim(2, 3)/cos(sim_ry), T_sim(3, 3)/cos(sim_ry));
+sim_rz = atan2(-T_sim(1, 2)/cos(sim_ry), T_sim(1, 1)/cos(sim_ry));