#+TITLE: Matlab Functions used for the NASS Project :DRAWER: #+STARTUP: overview #+PROPERTY: header-args:matlab :session *MATLAB* #+PROPERTY: header-args:matlab+ :comments org #+PROPERTY: header-args:matlab+ :results none #+PROPERTY: header-args:matlab+ :exports both #+PROPERTY: header-args:matlab+ :eval no-export #+PROPERTY: header-args:matlab+ :output-dir figs #+PROPERTY: header-args:matlab+ :tangle no #+PROPERTY: header-args:matlab+ :mkdirp yes #+PROPERTY: header-args:shell :eval no-export #+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}") #+PROPERTY: header-args:latex+ :imagemagick t :fit yes #+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 #+PROPERTY: header-args:latex+ :imoutoptions -quality 100 #+PROPERTY: header-args:latex+ :results raw replace :buffer no #+PROPERTY: header-args:latex+ :eval no-export #+PROPERTY: header-args:latex+ :exports both #+PROPERTY: header-args:latex+ :mkdirp yes #+PROPERTY: header-args:latex+ :output-dir figs :END: * describeNassSetup :PROPERTIES: :header-args:matlab+: :tangle ..//src/describeNassSetup.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> ** Function description :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab function [] = describeNassSetup() % describeNassSetup - % % Syntax: [] = describeNassSetup() % % Inputs: % - - % % Outputs: % - - #+end_src ** Simscape Configuration :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/conf_simscape.mat', 'conf_simscape'); #+end_src #+begin_src matlab fprintf('Simscape Configuration:\n'); if conf_simscape.type == 1 fprintf('- Gravity is included\n'); else fprintf('- Gravity is not included\n'); end fprintf('\n'); #+end_src ** Disturbances :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/nass_disturbances.mat', 'args'); #+end_src #+begin_src matlab 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'); #+end_src ** References :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/nass_references.mat', 'args'); #+end_src #+begin_src matlab 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/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'); #+end_src ** Controller :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/controller.mat', 'controller'); #+end_src #+begin_src matlab fprintf('Controller:\n'); fprintf('- %s\n', controller.name); fprintf('\n'); #+end_src ** Micro-Station :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc'); #+end_src #+begin_src matlab 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'); #+end_src ** Metrology :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/stages.mat', 'mirror'); #+end_src #+begin_src matlab fprintf('Reference Mirror:\n'); if mirror.type == 2; fprintf('- flexible fixation\n'); fprintf('- w = %.0f [Hz]\n', mirror.freq(1)); else fprintf('- rigidly attached to the nano-hexapod\n'); end fprintf('- m = %.0f [kg]\n', mirror.mass); fprintf('\n'); #+end_src ** Nano Hexapod :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/stages.mat', 'nano_hexapod'); #+end_src #+begin_src matlab fprintf('Nano Hexapod:\n'); if nano_hexapod.type == 0; fprintf('- no included\n'); elseif nano_hexapod.type == 1 || nano_hexapod.type == 3; fprintf('- rigid\n'); elseif nano_hexapod.type == 2; fprintf('- flexible\n'); fprintf('- Ki = %.0g [N/m]\n', nano_hexapod.Ki(1)); end fprintf('\n'); #+end_src ** Sample :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab load('./mat/stages.mat', 'sample'); #+end_src #+begin_src matlab fprintf('Sample:\n'); if sample.type == 0; fprintf('- no included\n'); elseif sample.type == 1 || sample.type == 3; fprintf('- rigid\n'); fprintf('- mass = %.0f [kg]\n', sample.mass); fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3)); elseif sample.type == 2; fprintf('- flexible\n'); fprintf('- mass = %.0f [kg]\n', sample.mass); fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3)); % fprintf('- Kt = %.0g, %.0g, %.0g [N/m]\n', sample.K(1), sample.K(2), sample.K(3)); % fprintf('- Kr = %.0g, %.0g, %.0g [Nm/rad]\n', sample.K(4), sample.K(5), sample.K(6)); fprintf('- wt(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass)); fprintf('- wr(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(4)/sample.inertia(1)), 1/2/pi*sqrt(sample.K(5)/sample.inertia(2)), 1/2/pi*sqrt(sample.K(6)/sample.inertia(3))); end fprintf('\n'); #+end_src * computeReferencePose :PROPERTIES: :header-args:matlab+: :tangle ../src/computeReferencePose.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:..//src/computeReferencePose.m][here]]. #+begin_src matlab 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 #+end_src * Compute the Sample Position Error w.r.t. the NASS :PROPERTIES: :header-args:matlab+: :tangle ../src/computeSampleError.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:..//src/computeSampleError.m][here]]. #+begin_src matlab function [MTr] = computeSampleError(WTm, WTr) % computeSampleError - % % Syntax: [MTr] = computeSampleError(WTm, WTr) % % Inputs: % - WTm - Homoegeneous transformation that represent the % wanted pose of the sample with respect to the granite % - WTr - Homoegeneous transformation that represent the % measured pose of the sample with respect to the granite % % Outputs: % - MTr - Homoegeneous transformation that represent the % wanted pose of the sample expressed in a frame % attached to the top platform of the nano-hexapod MTr = zeros(4,4); MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr; end #+end_src