#+TITLE: Matlab Functions used for the NASS Project
#+SETUPFILE: ./setup/org-setup-file.org

* describeNassSetup
:PROPERTIES:
:header-args:matlab+: :tangle ..//src/describeNassSetup.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:describeNassSetup>>


** 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/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');
#+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.actuators.K(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:
<<sec:computeReferencePose>>

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:
<<sec:computeSampleError>>

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