diff --git a/.SimulinkProject/Root.type.EntryPoints/63ca5d6e-cf1c-456e-953b-f40ba1181f39.type.EntryPoint.xml b/.SimulinkProject/Root.type.EntryPoints/63ca5d6e-cf1c-456e-953b-f40ba1181f39.type.EntryPoint.xml
new file mode 100644
index 0000000..f427c8f
--- /dev/null
+++ b/.SimulinkProject/Root.type.EntryPoints/63ca5d6e-cf1c-456e-953b-f40ba1181f39.type.EntryPoint.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.EntryPoints/a4ed91fd-d721-41c7-9bd0-c3d905ab3003.type.EntryPoint.xml b/.SimulinkProject/Root.type.EntryPoints/a4ed91fd-d721-41c7-9bd0-c3d905ab3003.type.EntryPoint.xml
deleted file mode 100644
index 491677b..0000000
--- a/.SimulinkProject/Root.type.EntryPoints/a4ed91fd-d721-41c7-9bd0-c3d905ab3003.type.EntryPoint.xml
+++ /dev/null
@@ -1,2 +0,0 @@
-
-
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.EntryPoints/bede1981-8866-468d-a7c7-1307e358108e.type.EntryPoint.xml b/.SimulinkProject/Root.type.EntryPoints/bede1981-8866-468d-a7c7-1307e358108e.type.EntryPoint.xml
deleted file mode 100644
index 9851cc3..0000000
--- a/.SimulinkProject/Root.type.EntryPoints/bede1981-8866-468d-a7c7-1307e358108e.type.EntryPoint.xml
+++ /dev/null
@@ -1,2 +0,0 @@
-
-
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.EntryPoints/d1613206-2f1a-4f6f-8d33-2e688c9cb3cd.type.EntryPoint.xml b/.SimulinkProject/Root.type.EntryPoints/d1613206-2f1a-4f6f-8d33-2e688c9cb3cd.type.EntryPoint.xml
new file mode 100644
index 0000000..9131ec2
--- /dev/null
+++ b/.SimulinkProject/Root.type.EntryPoints/d1613206-2f1a-4f6f-8d33-2e688c9cb3cd.type.EntryPoint.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.Files/src.type.File.xml b/.SimulinkProject/Root.type.Files/src.type.File.xml
new file mode 100644
index 0000000..1c0844e
--- /dev/null
+++ b/.SimulinkProject/Root.type.Files/src.type.File.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.Files/src.type.File/1.type.DIR_SIGNIFIER.xml b/.SimulinkProject/Root.type.Files/src.type.File/1.type.DIR_SIGNIFIER.xml
new file mode 100644
index 0000000..1c0844e
--- /dev/null
+++ b/.SimulinkProject/Root.type.Files/src.type.File/1.type.DIR_SIGNIFIER.xml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/.SimulinkProject/Root.type.Files/project_shutdown.m.type.File.xml b/.SimulinkProject/Root.type.Files/src.type.File/project_shutdown.m.type.File.xml
similarity index 100%
rename from .SimulinkProject/Root.type.Files/project_shutdown.m.type.File.xml
rename to .SimulinkProject/Root.type.Files/src.type.File/project_shutdown.m.type.File.xml
diff --git a/.SimulinkProject/Root.type.Files/project_startup.m.type.File.xml b/.SimulinkProject/Root.type.Files/src.type.File/project_startup.m.type.File.xml
similarity index 100%
rename from .SimulinkProject/Root.type.Files/project_startup.m.type.File.xml
rename to .SimulinkProject/Root.type.Files/src.type.File/project_startup.m.type.File.xml
diff --git a/.gitignore b/.gitignore
index b0075bc..1b79130 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,11 @@
+auto/
+*.tex
+
+**/figs/*.pdf
+**/figs/*.svg
+**/figs/*.tex
+
+
# Windows default autosave extension
*.asv
@@ -26,9 +34,3 @@ octave-workspace
# Simulink Cache
*.slxc
-
-# Custom
-Assemblage_grt_rtw/
-Figures/
-data/
-movies/
diff --git a/STEPS/axisc/Axisc_Mass.STEP b/STEPS/axisc/axisc_mass.STEP
similarity index 100%
rename from STEPS/axisc/Axisc_Mass.STEP
rename to STEPS/axisc/axisc_mass.STEP
diff --git a/active_damping/index.org b/active_damping/index.org
new file mode 100644
index 0000000..813ca76
--- /dev/null
+++ b/active_damping/index.org
@@ -0,0 +1,42 @@
+#+TITLE:
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ../index.html
+#+HTML_LINK_UP: ../index.html
+
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+
+#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+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 matlab/modal_frf_coh.m
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell :eval no-export
+
+#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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:
diff --git a/Analysis/analysis_perturbations.m b/analysis/analysis_perturbations.m
similarity index 100%
rename from Analysis/analysis_perturbations.m
rename to analysis/analysis_perturbations.m
diff --git a/Analysis/analyze_setpoint.m b/analysis/analyze_setpoint.m
similarity index 100%
rename from Analysis/analyze_setpoint.m
rename to analysis/analyze_setpoint.m
diff --git a/Analysis/effect_ground_motion.m b/analysis/effect_ground_motion.m
similarity index 100%
rename from Analysis/effect_ground_motion.m
rename to analysis/effect_ground_motion.m
diff --git a/Analysis/ground_motion_ol_cl.m b/analysis/ground_motion_ol_cl.m
similarity index 100%
rename from Analysis/ground_motion_ol_cl.m
rename to analysis/ground_motion_ol_cl.m
diff --git a/analysis/index.org b/analysis/index.org
new file mode 100644
index 0000000..3d23836
--- /dev/null
+++ b/analysis/index.org
@@ -0,0 +1,42 @@
+#+TITLE: Some analysis
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ../index.html
+#+HTML_LINK_UP: ../index.html
+
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+
+#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+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 matlab/modal_frf_coh.m
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell :eval no-export
+
+#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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:
diff --git a/Analysis/simulation_analysis.m b/analysis/simulation_analysis.m
similarity index 100%
rename from Analysis/simulation_analysis.m
rename to analysis/simulation_analysis.m
diff --git a/Analysis/tomography_experiment.m b/analysis/tomography_experiment.m
similarity index 100%
rename from Analysis/tomography_experiment.m
rename to analysis/tomography_experiment.m
diff --git a/Analysis/tomography_ol_cl.m b/analysis/tomography_ol_cl.m
similarity index 100%
rename from Analysis/tomography_ol_cl.m
rename to analysis/tomography_ol_cl.m
diff --git a/config.tex b/config.tex
deleted file mode 100644
index d4d41b1..0000000
--- a/config.tex
+++ /dev/null
@@ -1,30 +0,0 @@
-% ======================
-% New Commands
-% ======================
-% H Infini
-\newcommand{\hinf}{\mathcal{H}_\infty}
-% H 2
-\newcommand{\htwo}{\mathcal{H}_2}
-% Omega
-\newcommand{\w}{\omega}
-% H-Infinity Norm
-\newcommand{\hnorm}[1]{\|#1\|_\infty}
-% H-2 Norm
-\newcommand{\normtwo}[1]{\|#1\|_2}
-% Norm
-\newcommand{\abs}[1]{\vert #1 \vert}
-% Minimum Subscript
-\newcommand{\smin}{_{\text{min}}}
-% Maximum Subscript
-\newcommand{\smax}{_{\text{max}}}
-% Power Spectral Density
-\newcommand{\psd}[1]{\Phi_{#1}}
-% Amplitude Spectral Density
-\newcommand{\asd}[1]{\phi_{#1}}
-% Cumulative Power Spectrum
-\newcommand{\cps}[1]{\Sigma_{#1}}
-% Cumulative Amplitude Spectrum
-\newcommand{\cas}[1]{\sigma_{#1}}
-% Root Mean Square Value
-\newcommand{\rms}[1]{\sigma_{#1}}
-% ======================
diff --git a/control/index.org b/control/index.org
new file mode 100644
index 0000000..813ca76
--- /dev/null
+++ b/control/index.org
@@ -0,0 +1,42 @@
+#+TITLE:
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ../index.html
+#+HTML_LINK_UP: ../index.html
+
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+
+#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+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 matlab/modal_frf_coh.m
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell :eval no-export
+
+#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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:
diff --git a/demonstration/sim_nano_station_disp.slx b/demonstration/sim_nano_station_disp.slx
deleted file mode 100644
index 395d123..0000000
Binary files a/demonstration/sim_nano_station_disp.slx and /dev/null differ
diff --git a/hac_lac/index.org b/hac_lac/index.org
new file mode 100644
index 0000000..813ca76
--- /dev/null
+++ b/hac_lac/index.org
@@ -0,0 +1,42 @@
+#+TITLE:
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ../index.html
+#+HTML_LINK_UP: ../index.html
+
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+
+#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+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 matlab/modal_frf_coh.m
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell :eval no-export
+
+#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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:
diff --git a/identification/index.org b/identification/index.org
new file mode 100644
index 0000000..b03c35a
--- /dev/null
+++ b/identification/index.org
@@ -0,0 +1,64 @@
+#+TITLE: Identification
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ../index.html
+#+HTML_LINK_UP: ../index.html
+
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+
+#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+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 matlab/modal_frf_coh.m
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell :eval no-export
+
+#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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:
+
+* ZIP file containing the data and matlab files :ignore:
+#+begin_src bash :exports none :results none
+ if [ matlab/identification_micro_station.m -nt data/identification_micro_station.zip ]; then
+ cp matlab/identification_micro_station.m identification_micro_station.m;
+ zip data/identification_micro_station \
+ mat/data.mat \
+ identification_micro_station.m
+ rm identification_micro_station.m;
+ fi
+#+end_src
+
+#+begin_note
+ All the files (data and Matlab scripts) are accessible [[file:data/identification_micro_station.zip][here]].
+#+end_note
+
+
+* Identification of the micro-station
+
+* Plot the obtained transfer functions
+
+* Compare with the modal measurements
diff --git a/identification/sim_micro_station_id.slx b/identification/sim_micro_station_id.slx
index 19788e1..0e1205d 100644
Binary files a/identification/sim_micro_station_id.slx and b/identification/sim_micro_station_id.slx differ
diff --git a/identification/sim_nano_station_id.slx b/identification/sim_nano_station_id.slx
index 2dbea4e..176a604 100644
Binary files a/identification/sim_nano_station_id.slx and b/identification/sim_nano_station_id.slx differ
diff --git a/index.org b/index.org
index 02be79e..83e7030 100644
--- a/index.org
+++ b/index.org
@@ -6,6 +6,9 @@
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas
+#+HTML_LINK_HOME: ../index.html
+#+HTML_LINK_UP: ./index.html
+
#+HTML_HEAD:
#+HTML_HEAD:
#+HTML_HEAD:
@@ -15,4 +18,1727 @@
#+HTML_HEAD:
#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+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 matlab/modal_frf_coh.m
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell :eval no-export
+
+#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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:
+
+- [[file:identification/index.org][Identification of the Micro-Station]]
+- [[file:kinematics/index.org][Kinematics of the station]]
+- Control
+
+* Matlab Init :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+ <>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+ <>
+#+end_src
+
+* Simulink project
+The project can be opened using the =simulinkproject= function:
+
+#+begin_src matlab
+ simulinkproject('./');
+#+end_src
+
+When the project opens, a startup script is ran.
+The startup script is defined below and is exported to the =project_startup.m= script.
+#+begin_src matlab :eval no :tangle src/project_startup.m
+ %%
+ freqs = logspace(-1, 3, 1000);
+ save_fig = false;
+ save('./mat/config.mat', 'freqs', 'save_fig');
+
+ %%
+ project = simulinkproject;
+ projectRoot = project.RootFolder;
+
+ myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
+ myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
+
+ Simulink.fileGenControl('set',...
+ 'CacheFolder', myCacheFolder,...
+ 'CodeGenFolder', myCodeFolder,...
+ 'createDir', true);
+#+end_src
+
+When the project closes, it runs the =project_shutdown.m= script defined below.
+#+begin_src matlab :eval no :tangle src/project_shutdown.m
+ Simulink.fileGenControl('reset');
+#+end_src
+
+The project also permits to automatically add defined folder to the path when the project is opened.
+
+* Notes
+Step files are exported from SolidWorks and imported into Simscape.
+Each step file is then considered as a solid body.
+Inertia and mass matrices are computed from the density of the material and the geometry as defined by the step file.
+
+** Simscape files for identification
+|------------------------+----+----+----+------+------|
+| Simscape Name | Ty | Ry | Rz | Hexa | NASS |
+|------------------------+----+----+----+------+------|
+| id micro station | F | F | F | F | |
+| id nano station stages | F | F | F | F | F |
+| id nano station config | D | D | D | D | F |
+| control nano station | D | D | D | D | F |
+|------------------------+----+----+----+------+------|
+
+** Inputs
+*** Perturbations
+|----------+--------------------------------------+------+------|
+| Variable | Meaning | Size | Unit |
+|----------+--------------------------------------+------+------|
+| ~Dw~ | Ground motion | 3 | [m] |
+| ~Fg~ | External force applied on granite | 3 | [N] |
+| ~Fs~ | External force applied on the Sample | 3 | [N] |
+|----------+--------------------------------------+------+------|
+
+*** Measurement Noise
+|----------+---------+------+------|
+| Variable | Meaning | Size | Unit |
+|----------+---------+------+------|
+| | | | |
+|----------+---------+------+------|
+
+*** Control Inputs
+|----------+-------------------------------------------+------+----------|
+| Variable | Meaning | Size | Unit |
+|----------+-------------------------------------------+------+----------|
+| ~Fy~ | Actuation force for Ty | 1 | [N] |
+| ~Dy~ | Imposed displacement for Ty | 1 | [m] |
+|----------+-------------------------------------------+------+----------|
+| ~My~ | Actuation torque for Ry | 1 | [N.m] |
+| ~Ry~ | Imposed rotation for Ry | 1 | [rad] |
+|----------+-------------------------------------------+------+----------|
+| ~Mz~ | Actuation torque for Rz | 1 | [N.m] |
+| ~Rz~ | Imposed rotation for Rz | 1 | [rad] |
+|----------+-------------------------------------------+------+----------|
+| ~Fh~ | Actuation force/torque for hexapod (cart) | 6 | [N, N.m] |
+| ~Fhl~ | Actuation force/torque for hexapod (legs) | 6 | [N] |
+| ~Dh~ | Imposed position for hexapod (cart) | 6 | [m, rad] |
+|----------+-------------------------------------------+------+----------|
+| ~Rm~ | Position of the two masses | 2 | [rad] |
+|----------+-------------------------------------------+------+----------|
+| ~Fn~ | Actuation force for the NASS (cart) | 6 | [N, N.m] |
+| ~Fnl~ | Actuation force for the NASS's legs | 6 | [N] |
+| ~Dn~ | Imposed position for the NASS (cart) | 6 | [m, rad] |
+|----------+-------------------------------------------+------+----------|
+
+** Outputs
+
+|----------+---------------------------------------------+------+--------------|
+| Variable | Meaning | Size | Unit |
+|----------+---------------------------------------------+------+--------------|
+| ~Dgm~ | Absolute displacement of the granite | 3 | [m] |
+| ~Vgm~ | Absolute Velocity of the granite | 3 | [m/s] |
+|----------+---------------------------------------------+------+--------------|
+| ~Dym~ | Measured displacement of Ty | 1 | [m] |
+|----------+---------------------------------------------+------+--------------|
+| ~Rym~ | Measured rotation of Ry | 1 | [rad] |
+|----------+---------------------------------------------+------+--------------|
+| ~Rzm~ | Measured rotation of Rz | 1 | [rad] |
+|----------+---------------------------------------------+------+--------------|
+| ~Dhm~ | Measured position of hexapod (cart) | 6 | [m, rad] |
+|----------+---------------------------------------------+------+--------------|
+| ~Fnlm~ | Measured force of NASS's legs | 6 | [N] |
+| ~Dnlm~ | Measured elongation of NASS's legs | 6 | [m] |
+| ~Dnm~ | Measured position of NASS w.r.t NASS's base | 6 | [m, rad] |
+| ~Vnm~ | Measured absolute velocity of NASS platform | 6 | [m/s, rad/s] |
+| ~Vnlm~ | Measured absolute velocity of NASS's legs | 6 | [m/s] |
+|----------+---------------------------------------------+------+--------------|
+| ~Dsm~ | Position of Sample w.r.t. granite frame | 6 | [m, rad] |
+|----------+---------------------------------------------+------+--------------|
+
+* Simulink files
+Few different Simulink files are used:
+- kinematics
+- identification - micro station
+- identification - nano station
+- control
+
+* Simulink Library
+A simulink library is developed in order to share elements between the different simulink files.
+
+** inputs
+
+** nass_library
+
+** pos_error_wrt_nass_base
+
+** QuaternionToAngles
+
+** RotationMatrixToAngle
+
+* Scripts
+** Simulation Initialization
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/init_simulation.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab script is accessible [[file:src/init_simulation.m][here]].
+
+This script runs just before the simulation is started.
+It is used to load the simulation configuration and the controllers used for the simulation.
+
+#+begin_src matlab
+ %% Load all the data used for the simulation
+ load('./mat/sim_conf.mat');
+
+ %% Load Controller
+ load('./mat/controllers.mat');
+#+end_src
+
+* Functions
+** computePsdDispl
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/computePsdDispl.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/computePsdDispl.m][here]].
+
+#+begin_src matlab
+ function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
+ i_init = find(sys_data.time > t_init, 1);
+
+ han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
+ Fs = 1/sys_data.time(2);
+
+ [pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
+ [pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
+ [pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
+
+ [prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
+ [pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
+ [prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
+
+ psd_object = struct(...
+ 'f', f, ...
+ 'dx', pdx, ...
+ 'dy', pdy, ...
+ 'dz', pdz, ...
+ 'rx', prx, ...
+ 'ry', pry, ...
+ 'rz', prz);
+ end
+#+end_src
+** computeSetpoint
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/computeSetpoint.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/computeSetpoint.m][here]].
+
+#+begin_src matlab
+ function setpoint = computeSetpoint(ty, ry, rz)
+ %%
+ setpoint = zeros(6, 1);
+
+ %% Ty
+ Ty = [1 0 0 0 ;
+ 0 1 0 ty ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+ % Tyinv = [1 0 0 0 ;
+ % 0 1 0 -ty ;
+ % 0 0 1 0 ;
+ % 0 0 0 1 ];
+
+ %% Ry
+ Ry = [ cos(ry) 0 sin(ry) 0 ;
+ 0 1 0 0 ;
+ -sin(ry) 0 cos(ry) 0 ;
+ 0 0 0 1 ];
+
+ % TMry = Ty*Ry*Tyinv;
+
+ %% Rz
+ Rz = [cos(rz) -sin(rz) 0 0 ;
+ sin(rz) cos(rz) 0 0 ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+ % TMrz = Ty*TMry*Rz*TMry'*Tyinv;
+
+ %% All stages
+ % TM = TMrz*TMry*Ty;
+
+ TM = Ty*Ry*Rz;
+
+ [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
+
+ setpoint(1:3) = TM(1:3, 4);
+ setpoint(4:6) = [thetax, thetay, thetaz];
+
+ %% Custom Functions
+ function [thetax, thetay, thetaz] = RM2angle(R)
+ if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+ thetay = -asin(R(3, 1));
+ thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+ thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+ else
+ thetaz = 0;
+ if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+ thetay = pi/2;
+ thetax = thetaz + atan2(R(1, 2), R(1, 3));
+ else
+ thetay = -pi/2;
+ thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+ end
+ end
+ end
+ end
+#+end_src
+** converErrorBasis
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/converErrorBasis.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/converErrorBasis.m][here]].
+
+#+begin_src matlab
+ function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
+ % convertErrorBasis -
+ %
+ % Syntax: convertErrorBasis(p_error, ty, ry, rz)
+ %
+ % Inputs:
+ % - p_error - Position error of the sample w.r.t. the granite [m, rad]
+ % - ty - Measured translation of the Ty stage [m]
+ % - ry - Measured rotation of the Ry stage [rad]
+ % - rz - Measured rotation of the Rz stage [rad]
+ %
+ % Outputs:
+ % - P_nass - Position error of the sample w.r.t. the NASS base [m]
+ % - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
+ %
+ % Example:
+ %
+
+ %% If line vector => column vector
+ if size(pos, 2) == 6
+ pos = pos';
+ end
+
+ if size(setpoint, 2) == 6
+ setpoint = setpoint';
+ end
+
+ %% Position of the sample in the frame fixed to the Granite
+ P_granite = [pos(1:3); 1]; % Position [m]
+ R_granite = [setpoint(1:3); 1]; % Reference [m]
+
+ %% Transformation matrices of the stages
+ % T-y
+ TMty = [1 0 0 0 ;
+ 0 1 0 ty ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+ % R-y
+ TMry = [ cos(ry) 0 sin(ry) 0 ;
+ 0 1 0 0 ;
+ -sin(ry) 0 cos(ry) 0 ;
+ 0 0 0 1 ];
+
+ % R-z
+ TMrz = [cos(rz) -sin(rz) 0 0 ;
+ sin(rz) cos(rz) 0 0 ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+ %% Compute Point coordinates in the new reference fixed to the NASS base
+ % P_nass = TMrz*TMry*TMty*P_granite;
+ P_nass = TMrz\TMry\TMty\P_granite;
+ R_nass = TMrz\TMry\TMty\R_granite;
+
+ dx = R_nass(1)-P_nass(1);
+ dy = R_nass(2)-P_nass(2);
+ dz = R_nass(3)-P_nass(3);
+
+ %% Compute new basis vectors linked to the NASS base
+ % ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
+ % ux_nass = ux_nass(1:3);
+ % uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
+ % uy_nass = uy_nass(1:3);
+ % uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
+ % uz_nass = uz_nass(1:3);
+
+ ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
+ ux_nass = ux_nass(1:3);
+ uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
+ uy_nass = uy_nass(1:3);
+ uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
+ uz_nass = uz_nass(1:3);
+
+ %% Rotations error w.r.t. granite Frame
+ % Rotations error w.r.t. granite Frame
+ rx_nass = pos(4);
+ ry_nass = pos(5);
+ rz_nass = pos(6);
+
+ % Rotation matrices of the Sample w.r.t. the Granite
+ Mrx_error = [1 0 0 ;
+ 0 cos(-rx_nass) -sin(-rx_nass) ;
+ 0 sin(-rx_nass) cos(-rx_nass)];
+
+ Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
+ 0 1 0 ;
+ -sin(-ry_nass) 0 cos(-ry_nass)];
+
+ Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
+ sin(-rz_nass) cos(-rz_nass) 0 ;
+ 0 0 1];
+
+ % Rotation matrix of the Sample w.r.t. the Granite
+ Mr_error = Mrz_error*Mry_error*Mrx_error;
+
+ %% Use matrix to solve
+ R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample
+
+ [thetax, thetay, thetaz] = RM2angle(R);
+
+ error_nass = [dx; dy; dz; thetax; thetay; thetaz];
+
+ %% Custom Functions
+ function [thetax, thetay, thetaz] = RM2angle(R)
+ if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+ thetay = -asin(R(3, 1));
+ % thetaybis = pi-thetay;
+ thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+ % thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
+ thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+ % thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
+ else
+ thetaz = 0;
+ if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+ thetay = pi/2;
+ thetax = thetaz + atan2(R(1, 2), R(1, 3));
+ else
+ thetay = -pi/2;
+ thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+ end
+ end
+ end
+
+ end
+#+end_src
+** generateDiagPidControl
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/generateDiagPidControl.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/generateDiagPidControl.m][here]].
+
+#+begin_src matlab
+ function [K] = generateDiagPidControl(G, fs)
+ %%
+ pid_opts = pidtuneOptions(...
+ 'PhaseMargin', 50, ...
+ 'DesignFocus', 'disturbance-rejection');
+
+ %%
+ K = tf(zeros(6));
+
+ for i = 1:6
+ input_name = G.InputName(i);
+ output_name = G.OutputName(i);
+ K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
+ end
+
+ K.InputName = G.OutputName;
+ K.OutputName = G.InputName;
+ end
+#+end_src
+** identifyPlant
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/identifyPlant.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/identifyPlant.m][here]].
+
+#+begin_src matlab
+ function [sys] = identifyPlant(opts_param)
+ %% Default values for opts
+ opts = struct();
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %% Options for Linearized
+ options = linearizeOptions;
+ options.SampleTime = 0;
+
+ %% Name of the Simulink File
+ mdl = 'sim_nano_station_id';
+
+ %% Input/Output definition
+ io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS
+ io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion
+ io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample
+ io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs
+ io(5) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
+ io(6) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
+ io(7) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
+ io(8) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
+
+ %% Run the linearization
+ G = linearize(mdl, io, 0);
+ G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
+ 'Dgx', 'Dgy', 'Dgz', ...
+ 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
+ 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
+ G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
+ 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
+ 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
+ 'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'};
+
+ %% Create the sub transfer functions
+ % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
+ sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
+ % From ground motion to Sample displacement
+ sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}));
+ % From direct forces applied on the sample to displacement of the sample
+ sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}));
+ % From forces applied on NASS's legs to force sensor in each leg
+ sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
+ % From forces applied on NASS's legs to displacement of each leg
+ sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
+ % From forces applied on NASS's legs to displacement of each leg
+ sys.G_plant = minreal(G({'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
+ end
+#+end_src
+** runSimulation
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/runSimulation.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/runSimulation.m][here]].
+
+#+begin_src matlab
+ function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
+ %% Load the controller and save it for the simulation
+ if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
+ K_obj = load('./mat/K_fb.mat');
+ K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
+ save('./mat/controllers.mat', 'K');
+ elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
+ K_obj = load('./mat/K_fb_iff.mat');
+ K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
+ save('./mat/controllers.mat', 'K');
+ elseif strcmp(ctrl_type, 'ol')
+ K = tf(zeros(6)); %#ok
+ save('./mat/controllers.mat', 'K');
+ else
+ error('ctrl_type should be cl or ol');
+ end
+
+ %% Active Damping
+ if strcmp(act_damp, 'iff')
+ K_iff_crit = load('./mat/K_iff_crit.mat');
+ K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok
+ save('./mat/controllers.mat', 'K_iff', '-append');
+ elseif strcmp(act_damp, 'none')
+ K_iff = tf(zeros(6)); %#ok
+ save('./mat/controllers.mat', 'K_iff', '-append');
+ end
+
+ %%
+ if strcmp(sys_name, 'pz')
+ initializeNanoHexapod(struct('actuator', 'piezo'));
+ elseif strcmp(sys_name, 'vc')
+ initializeNanoHexapod(struct('actuator', 'lorentz'));
+ else
+ error('sys_name should be pz or vc');
+ end
+
+ if strcmp(sys_mass, 'light')
+ initializeSample(struct('mass', 1));
+ elseif strcmp(sys_mass, 'heavy')
+ initializeSample(struct('mass', 50));
+ else
+ error('sys_mass should be light or heavy');
+ end
+
+ %% Run the simulation
+ sim('sim_nano_station_ctrl.slx');
+
+ %% Split the Dsample matrix into vectors
+ [Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok
+ time = Dsample.Time; %#ok
+
+ %% Save the result
+ filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp);
+ save(sprintf('./mat/%s.mat', filename), ...
+ 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+ end
+#+end_src
+* Initialize Elements
+** Simulation Configuration
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeSimConf.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeSimConf.m][here]].
+
+#+begin_src matlab
+ function [] = initializeSimConf(opts_param)
+ %% Default values for opts
+ opts = struct('Ts', 1e-4, ... % Sampling time [s]
+ 'Tsim', 10, ... % Simulation time [s]
+ 'cl_time', 0, ... % Close Loop time [s]
+ 'gravity', false ... % Gravity along the z axis
+ );
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %%
+ sim_conf = struct();
+
+ %%
+ sim_conf.Ts = opts.Ts;
+ sim_conf.Tsim = opts.Tsim;
+ sim_conf.cl_time = opts.cl_time;
+
+ %% Gravity
+ if opts.gravity
+ sim_conf.g = -9.8; %#ok
+ else
+ sim_conf.g = 0; %#ok
+ end
+
+ %% Save
+ save('./mat/sim_conf.mat', 'sim_conf');
+ end
+#+end_src
+
+** Experiment
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeExperiment.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeExperiment.m][here]].
+
+#+begin_src matlab
+ function [] = initializeExperiment(exp_name, sys_mass)
+ if strcmp(exp_name, 'tomography')
+ opts_sim = struct(...
+ 'Tsim', 5, ...
+ 'cl_time', 5 ...
+ );
+ initializeSimConf(opts_sim);
+
+ if strcmp(sys_mass, 'light')
+ opts_inputs = struct(...
+ 'Dw', true, ...
+ 'Rz', 60 ... % rpm
+ );
+ elseif strcpm(sys_mass, 'heavy')
+ opts_inputs = struct(...
+ 'Dw', true, ...
+ 'Rz', 1 ... % rpm
+ );
+ else
+ error('sys_mass should be light or heavy');
+ end
+
+ initializeInputs(opts_inputs);
+ else
+ error('exp_name is only configured for tomography');
+ end
+ end
+#+end_src
+
+** Inputs
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeInputs.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeInputs.m][here]].
+
+#+begin_src matlab
+ function [inputs] = initializeInputs(opts_param)
+ %% Default values for opts
+ opts = struct( ...
+ 'Dw', false, ...
+ 'Dy', false, ...
+ 'Ry', false, ...
+ 'Rz', false, ...
+ 'Dh', false, ...
+ 'Rm', false, ...
+ 'Dn', false ...
+ );
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %% Load Sampling Time and Simulation Time
+ load('./mat/sim_conf.mat', 'sim_conf');
+
+ %% Define the time vector
+ t = 0:sim_conf.Ts:sim_conf.Tsim;
+
+ %% Ground motion - Dw
+ if islogical(opts.Dw) && opts.Dw == true
+ load('./mat/perturbations.mat', 'Wxg');
+ Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3);
+ Dw(:, 1) = lsim(Wxg, Dw(:, 1), t);
+ Dw(:, 2) = lsim(Wxg, Dw(:, 2), t);
+ Dw(:, 3) = lsim(Wxg, Dw(:, 3), t);
+ elseif islogical(opts.Dw) && opts.Dw == false
+ Dw = zeros(length(t), 3);
+ else
+ Dw = opts.Dw;
+ end
+
+ %% Translation stage - Dy
+ if islogical(opts.Dy) && opts.Dy == true
+ Dy = zeros(length(t), 1);
+ elseif islogical(opts.Dy) && opts.Dy == false
+ Dy = zeros(length(t), 1);
+ else
+ Dy = opts.Dy;
+ end
+
+ %% Tilt Stage - Ry
+ if islogical(opts.Ry) && opts.Ry == true
+ Ry = 3*(2*pi/360)*sin(2*pi*0.2*t);
+ elseif islogical(opts.Ry) && opts.Ry == false
+ Ry = zeros(length(t), 1);
+ elseif isnumeric(opts.Ry) && length(opts.Ry) == 1
+ Ry = opts.Ry*(2*pi/360)*ones(length(t), 1);
+ else
+ Ry = opts.Ry;
+ end
+
+ %% Spindle - Rz
+ if islogical(opts.Rz) && opts.Rz == true
+ Rz = 2*pi*0.5*t;
+ elseif islogical(opts.Rz) && opts.Rz == false
+ Rz = zeros(length(t), 1);
+ elseif isnumeric(opts.Rz) && length(opts.Rz) == 1
+ Rz = opts.Rz*(2*pi/60)*t;
+ else
+ Rz = opts.Rz;
+ end
+
+ %% Micro Hexapod - Dh
+ if islogical(opts.Dh) && opts.Dh == true
+ Dh = zeros(length(t), 6);
+ elseif islogical(opts.Dh) && opts.Dh == false
+ Dh = zeros(length(t), 6);
+ else
+ Dh = opts.Dh;
+ end
+
+ %% Axis Compensation - Rm
+ if islogical(opts.Rm)
+ Rm = zeros(length(t), 2);
+ Rm(:, 2) = pi*ones(length(t), 1);
+ else
+ Rm = opts.Rm;
+ end
+
+ %% Nano Hexapod - Dn
+ if islogical(opts.Dn) && opts.Dn == true
+ Dn = zeros(length(t), 6);
+ elseif islogical(opts.Dn) && opts.Dn == false
+ Dn = zeros(length(t), 6);
+ else
+ Dn = opts.Dn;
+ end
+
+ %% Setpoint - Ds
+ Ds = zeros(length(t), 6);
+ for i = 1:length(t)
+ Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i));
+ end
+
+ %% External Forces applied on the Granite
+ Fg = zeros(length(t), 3);
+
+ %% External Forces applied on the Sample
+ Fs = zeros(length(t), 6);
+
+ %% Create the input Structure that will contain all the inputs
+ inputs = struct( ...
+ 'Ts', sim_conf.Ts, ...
+ 'Dw', timeseries(Dw, t), ...
+ 'Dy', timeseries(Dy, t), ...
+ 'Ry', timeseries(Ry, t), ...
+ 'Rz', timeseries(Rz, t), ...
+ 'Dh', timeseries(Dh, t), ...
+ 'Rm', timeseries(Rm, t), ...
+ 'Dn', timeseries(Dn, t), ...
+ 'Ds', timeseries(Ds, t), ...
+ 'Fg', timeseries(Fg, t), ...
+ 'Fs', timeseries(Fs, t) ...
+ );
+
+ %% Save
+ save('./mat/inputs.mat', 'inputs');
+
+ %% Custom Functions
+ function setpoint = computeSetpoint(ty, ry, rz)
+ %%
+ setpoint = zeros(6, 1);
+
+ %% Ty
+ TMTy = [1 0 0 0 ;
+ 0 1 0 ty ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+ %% Ry
+ TMRy = [ cos(ry) 0 sin(ry) 0 ;
+ 0 1 0 0 ;
+ -sin(ry) 0 cos(ry) 0 ;
+ 0 0 0 1 ];
+
+ %% Rz
+ TMRz = [cos(rz) -sin(rz) 0 0 ;
+ sin(rz) cos(rz) 0 0 ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+ %% All stages
+ TM = TMTy*TMRy*TMRz;
+
+ [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
+
+ setpoint(1:3) = TM(1:3, 4);
+ setpoint(4:6) = [thetax, thetay, thetaz];
+
+ %% Custom Functions
+ function [thetax, thetay, thetaz] = RM2angle(R)
+ if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+ thetay = -asin(R(3, 1));
+ thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+ thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+ else
+ thetaz = 0;
+ if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+ thetay = pi/2;
+ thetax = thetaz + atan2(R(1, 2), R(1, 3));
+ else
+ thetay = -pi/2;
+ thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+ end
+ end
+ end
+ end
+ end
+#+end_src
+
+** Ground
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeGround.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeGround.m][here]].
+
+#+begin_src matlab
+function [ground] = initializeGround()
+ %%
+ ground = struct();
+
+ ground.shape = [2, 2, 0.5]; % [m]
+ ground.density = 2800; % [kg/m3]
+ ground.color = [0.5, 0.5, 0.5];
+
+ %% Save
+ save('./mat/stages.mat', 'ground', '-append');
+end
+#+end_src
+
+** Granite
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeGranite.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeGranite.m][here]].
+
+#+begin_src matlab
+ function [granite] = initializeGranite()
+ %%
+ granite = struct();
+
+ %% Static Properties
+ granite.density = 2800; % [kg/m3]
+ granite.volume = 0.72; % [m3] TODO - should
+ granite.mass = granite.density*granite.volume; % [kg]
+ granite.color = [1 1 1];
+ granite.STEP = './STEPS/granite/granite.STEP';
+
+ %% Dynamical Properties
+ granite.k.x = 1e8; % [N/m]
+ granite.c.x = 1e4; % [N/(m/s)]
+
+ granite.k.y = 1e8; % [N/m]
+ granite.c.y = 1e4; % [N/(m/s)]
+
+ granite.k.z = 1e8; % [N/m]
+ granite.c.z = 1e4; % [N/(m/s)]
+
+ %% Positioning parameters
+ granite.sample_pos = 0.8; % Z-offset for the initial position of the sample [m]
+
+ %% Save
+ save('./mat/stages.mat', 'granite', '-append');
+ end
+#+end_src
+
+** Translation Stage
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeTy.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeTy.m][here]].
+
+#+begin_src matlab
+ function [ty] = initializeTy(opts_param)
+ %% Default values for opts
+ opts = struct('rigid', false);
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %%
+ ty = struct();
+
+ %% Y-Translation - Static Properties
+ % Ty Granite frame
+ ty.granite_frame.density = 7800; % [kg/m3]
+ ty.granite_frame.color = [0.753 1 0.753];
+ ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP';
+ % Guide Translation Ty
+ ty.guide.density = 7800; % [kg/m3]
+ ty.guide.color = [0.792 0.820 0.933];
+ ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP';
+ % Ty - Guide_Translation12
+ ty.guide12.density = 7800; % [kg/m3]
+ ty.guide12.color = [0.792 0.820 0.933];
+ ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP';
+ % Ty - Guide_Translation11
+ ty.guide11.density = 7800; % [kg/m3]
+ ty.guide11.color = [0.792 0.820 0.933];
+ ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP';
+ % Ty - Guide_Translation22
+ ty.guide22.density = 7800; % [kg/m3]
+ ty.guide22.color = [0.792 0.820 0.933];
+ ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP';
+ % Ty - Guide_Translation21
+ ty.guide21.density = 7800; % [kg/m3]
+ ty.guide21.color = [0.792 0.820 0.933];
+ ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP';
+ % Ty - Plateau translation
+ ty.frame.density = 7800; % [kg/m3]
+ ty.frame.color = [0.792 0.820 0.933];
+ ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP';
+ % Ty Stator Part
+ ty.stator.density = 5400; % [kg/m3]
+ ty.stator.color = [0.792 0.820 0.933];
+ ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP';
+ % Ty Rotor Part
+ ty.rotor.density = 5400; % [kg/m3]
+ ty.rotor.color = [0.792 0.820 0.933];
+ ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
+
+ ty.m = 250; % TODO [kg]
+
+ %% Y-Translation - Dynamicals Properties
+ if opts.rigid
+ ty.k.ax = 1e10; % Axial Stiffness for each of the 4 guidance (y) [N/m]
+ else
+ ty.k.ax = 1e7/4; % Axial Stiffness for each of the 4 guidance (y) [N/m]
+ end
+ ty.k.rad = 9e9/4; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
+
+ ty.c.ax = 100*(1/5)*sqrt(ty.k.ax/ty.m);
+ ty.c.rad = 100*(1/5)*sqrt(ty.k.rad/ty.m);
+
+ %% Save
+ save('./mat/stages.mat', 'ty', '-append');
+ end
+#+end_src
+
+** Tilt Stage
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeRy.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeRy.m][here]].
+
+#+begin_src matlab
+ function [ry] = initializeRy(opts_param)
+ %% Default values for opts
+ opts = struct('rigid', false);
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %%
+ ry = struct();
+
+ %% Tilt Stage - Static Properties
+ % Ry - Guide for the tilt stage
+ ry.guide.density = 7800; % [kg/m3]
+ ry.guide.color = [0.792 0.820 0.933];
+ ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP';
+ % Ry - Rotor of the motor
+ ry.rotor.density = 2400; % [kg/m3]
+ ry.rotor.color = [0.792 0.820 0.933];
+ ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP';
+ % Ry - Motor
+ ry.motor.density = 3200; % [kg/m3]
+ ry.motor.color = [0.792 0.820 0.933];
+ ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP';
+ % Ry - Plateau Tilt
+ ry.stage.density = 7800; % [kg/m3]
+ ry.stage.color = [0.792 0.820 0.933];
+ ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP';
+
+ ry.m = 200; % TODO [kg]
+
+ %% Tilt Stage - Dynamical Properties
+ if opts.rigid
+ ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg]
+ else
+ ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg]
+ end
+
+ ry.k.h = 357e6/4; % Stiffness in the direction of the guidance [N/m]
+ ry.k.rad = 555e6/4; % Stiffness in the top direction [N/m]
+ ry.k.rrad = 238e6/4; % Stiffness in the side direction [N/m]
+
+ ry.c.h = 10*(1/5)*sqrt(ry.k.h/ry.m);
+ ry.c.rad = 10*(1/5)*sqrt(ry.k.rad/ry.m);
+ ry.c.rrad = 10*(1/5)*sqrt(ry.k.rrad/ry.m);
+ ry.c.tilt = 10*(1/1)*sqrt(ry.k.tilt/ry.m);
+
+ %% Positioning parameters
+ ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m]
+
+ %% Save
+ save('./mat/stages.mat', 'ry', '-append');
+ end
+#+end_src
+
+** Spindle
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeRz.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeRz.m][here]].
+
+#+begin_src matlab
+ function [rz] = initializeRz(opts_param)
+ %% Default values for opts
+ opts = struct('rigid', false);
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %%
+ rz = struct();
+
+ %% Spindle - Static Properties
+ % Spindle - Slip Ring
+ rz.slipring.density = 7800; % [kg/m3]
+ rz.slipring.color = [0.792 0.820 0.933];
+ rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP';
+ % Spindle - Rotor
+ rz.rotor.density = 7800; % [kg/m3]
+ rz.rotor.color = [0.792 0.820 0.933];
+ rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP';
+ % Spindle - Stator
+ rz.stator.density = 7800; % [kg/m3]
+ rz.stator.color = [0.792 0.820 0.933];
+ rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
+
+ % Estimated mass of the mooving part
+ rz.m = 250; % [kg]
+
+ %% Spindle - Dynamical Properties
+ % Estimated stiffnesses
+ rz.k.ax = 2e9; % Axial Stiffness [N/m]
+ rz.k.rad = 7e8; % Radial Stiffness [N/m]
+ rz.k.rot = 100e6*2*pi/360; % Rotational Stiffness [N*m/deg]
+
+ if opts.rigid
+ rz.k.tilt = 1e10; % Vertical Rotational Stiffness [N*m/deg]
+ else
+ rz.k.tilt = 1e2; % TODO what value should I put? [N*m/deg]
+ end
+
+ % TODO
+ rz.c.ax = 2*sqrt(rz.k.ax/rz.m);
+ rz.c.rad = 2*sqrt(rz.k.rad/rz.m);
+ rz.c.tilt = 100*sqrt(rz.k.tilt/rz.m);
+ rz.c.rot = 100*sqrt(rz.k.rot/rz.m);
+
+ %% Save
+ save('./mat/stages.mat', 'rz', '-append');
+ end
+#+end_src
+
+** Micro Hexapod
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeMicroHexapod.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeMicroHexapod.m][here]].
+
+#+begin_src matlab
+ function [micro_hexapod] = initializeMicroHexapod(opts_param)
+ %% Default values for opts
+ opts = struct();
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %% Stewart Object
+ micro_hexapod = struct();
+ micro_hexapod.h = 350; % Total height of the platform [mm]
+ % micro_hexapod.jacobian = 269.26; % Distance from the top platform to the Jacobian point [mm]
+ micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm]
+
+ %% Bottom Plate - Mechanical Design
+ BP = struct();
+
+ BP.rad.int = 110; % Internal Radius [mm]
+ BP.rad.ext = 207.5; % External Radius [mm]
+ BP.thickness = 26; % Thickness [mm]
+ BP.leg.rad = 175.5; % Radius where the legs articulations are positionned [mm]
+ BP.leg.ang = 9.5; % Angle Offset [deg]
+ BP.density = 8000; % Density of the material [kg/m^3]
+ BP.color = [0.6 0.6 0.6]; % Color [rgb]
+ BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
+
+ %% Top Plate - Mechanical Design
+ TP = struct();
+
+ TP.rad.int = 82; % Internal Radius [mm]
+ TP.rad.ext = 150; % Internal Radius [mm]
+ TP.thickness = 26; % Thickness [mm]
+ TP.leg.rad = 118; % Radius where the legs articulations are positionned [mm]
+ TP.leg.ang = 12.1; % Angle Offset [deg]
+ TP.density = 8000; % Density of the material [kg/m^3]
+ TP.color = [0.6 0.6 0.6]; % Color [rgb]
+ TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
+
+ %% Struts
+ Leg = struct();
+
+ Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
+ Leg.k.ax = 5e7; % Stiffness of each leg [N/m]
+ Leg.ksi.ax = 3; % Maximum amplification at resonance []
+ Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm]
+ Leg.rad.top = 17; % Radius of the cylinder of the top part [mm]
+ Leg.density = 8000; % Density of the material [kg/m^3]
+ Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
+ Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
+
+ Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
+ Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
+ Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
+ Leg = updateDamping(Leg);
+
+
+ %% Sphere
+ SP = struct();
+
+ SP.height.bottom = 27; % [mm]
+ SP.height.top = 27; % [mm]
+ SP.density.bottom = 8000; % [kg/m^3]
+ SP.density.top = 8000; % [kg/m^3]
+ SP.color.bottom = [0.6 0.6 0.6]; % [rgb]
+ SP.color.top = [0.6 0.6 0.6]; % [rgb]
+ SP.k.ax = 0; % [N*m/deg]
+ SP.ksi.ax = 10;
+
+ SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
+ SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
+ SP.rad.bottom = Leg.sphere.bottom; % [mm]
+ SP.rad.top = Leg.sphere.top; % [mm]
+ SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
+
+ SP = updateDamping(SP);
+
+ %%
+ Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
+ Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
+
+ %%
+ micro_hexapod.BP = BP;
+ micro_hexapod.TP = TP;
+ micro_hexapod.Leg = Leg;
+ micro_hexapod.SP = SP;
+
+ %%
+ micro_hexapod = initializeParameters(micro_hexapod);
+
+ %% Save
+ save('./mat/stages.mat', 'micro_hexapod', '-append');
+
+ %%
+ function [element] = updateDamping(element)
+ field = fieldnames(element.k);
+ for i = 1:length(field)
+ element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
+ end
+ end
+
+ %%
+ function [stewart] = initializeParameters(stewart)
+ %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
+ stewart.pos_base = zeros(6, 3);
+ stewart.pos_top = zeros(6, 3);
+
+ alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
+ alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
+
+ height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
+
+ radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
+ radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
+
+ for i = 1:3
+ % base points
+ angle_m_b = (2*pi/3)* (i-1) - alpha_b;
+ angle_p_b = (2*pi/3)* (i-1) + alpha_b;
+ stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
+ stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
+
+ % top points
+ % Top points are 60 degrees offset
+ angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
+ angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
+ stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
+ stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
+ end
+
+ % permute pos_top points so that legs are end points of base and top points
+ stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
+ stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
+
+ %% leg vectors
+ legs = stewart.pos_top - stewart.pos_base;
+ leg_length = zeros(6, 1);
+ leg_vectors = zeros(6, 3);
+ for i = 1:6
+ leg_length(i) = norm(legs(i,:));
+ leg_vectors(i,:) = legs(i,:) / leg_length(i);
+ end
+
+ stewart.Leg.lenght = 1000*leg_length(1)/1.5;
+ stewart.Leg.shape.bot = [0 0; ...
+ stewart.Leg.rad.bottom 0; ...
+ stewart.Leg.rad.bottom stewart.Leg.lenght; ...
+ stewart.Leg.rad.top stewart.Leg.lenght; ...
+ stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
+ 0 0.2*stewart.Leg.lenght];
+
+ %% Calculate revolute and cylindrical axes
+ rev1 = zeros(6, 3);
+ rev2 = zeros(6, 3);
+ cyl1 = zeros(6, 3);
+ for i = 1:6
+ rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
+ rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
+
+ rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
+ rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
+
+ cyl1(i,:) = leg_vectors(i,:);
+ end
+
+
+ %% Coordinate systems
+ stewart.lower_leg = struct('rotation', eye(3));
+ stewart.upper_leg = struct('rotation', eye(3));
+
+ for i = 1:6
+ stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+ stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+ end
+
+ %% Position Matrix
+ stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
+
+ %% Compute Jacobian Matrix
+ aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
+ stewart.J = getJacobianMatrix(leg_vectors', aa');
+ end
+
+ function J = getJacobianMatrix(RM,M_pos_base)
+ % RM: [3x6] unit vector of each leg in the fixed frame
+ % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
+ J = zeros(6);
+ J(:, 1:3) = RM';
+ J(:, 4:6) = cross(M_pos_base, RM)';
+ end
+ end
+#+end_src
+
+** Center of gravity compensation
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeAxisc.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeAxisc.m][here]].
+
+#+begin_src matlab
+ function [axisc] = initializeAxisc()
+ %%
+ axisc = struct();
+
+ %% Axis Compensator - Static Properties
+ % Structure
+ axisc.structure.density = 3400; % [kg/m3]
+ axisc.structure.color = [0.792 0.820 0.933];
+ axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP';
+ % Wheel
+ axisc.wheel.density = 2700; % [kg/m3]
+ axisc.wheel.color = [0.753 0.753 0.753];
+ axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP';
+ % Mass
+ axisc.mass.density = 7800; % [kg/m3]
+ axisc.mass.color = [0.792 0.820 0.933];
+ axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP';
+ % Gear
+ axisc.gear.density = 7800; % [kg/m3]
+ axisc.gear.color = [0.792 0.820 0.933];
+ axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP';
+
+ axisc.m = 40; % TODO [kg]
+
+ %% Axis Compensator - Dynamical Properties
+ axisc.k.ax = 1; % TODO [N*m/deg)]
+ axisc.c.ax = (1/1)*sqrt(axisc.k.ax/axisc.m);
+
+ %% Save
+ save('./mat/stages.mat', 'axisc', '-append');
+ end
+#+end_src
+** Mirror
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeMirror.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeMirror.m][here]].
+
+#+begin_src matlab
+ function [] = initializeMirror(opts_param)
+ %% Default values for opts
+ opts = struct(...
+ 'shape', 'spherical', ... % spherical or conical
+ 'angle', 45 ...
+ );
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %%
+ mirror = struct();
+ mirror.h = 50; % height of the mirror [mm]
+ mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
+ mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
+ mirror.support_rad = 100; % radius of the support plate [mm]
+ mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
+ mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
+
+ mirror.density = 2400; % Density of the mirror [kg/m3]
+ mirror.color = [0.4 1.0 1.0]; % Color of the mirror
+
+ mirror.cone_length = mirror.rad*tand(opts.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
+
+ %% Shape
+ mirror.shape = [...
+ 0 mirror.h-mirror.thickness
+ mirror.hole_rad mirror.h-mirror.thickness; ...
+ mirror.hole_rad 0; ...
+ mirror.rad 0 ...
+ ];
+
+ if strcmp(opts.shape, 'spherical')
+ mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]
+
+ for z = linspace(0, mirror.h, 101)
+ mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
+ end
+ elseif strcmp(opts.shape, 'conical')
+ mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.angle) mirror.h];
+ else
+ error('Shape should be either conical or spherical');
+ end
+
+ mirror.shape = [mirror.shape; 0 mirror.h];
+
+ %% Save
+ save('./mat/stages.mat', 'mirror', '-append');
+ end
+#+end_src
+
+** Nano Hexapod
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeNanoHexapod.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeNanoHexapod.m][here]].
+
+#+begin_src matlab
+ function [nano_hexapod] = initializeNanoHexapod(opts_param)
+ %% Default values for opts
+ opts = struct('actuator', 'piezo');
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ opts.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %% Stewart Object
+ nano_hexapod = struct();
+ nano_hexapod.h = 90; % Total height of the platform [mm]
+ nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm]
+ % nano_hexapod.jacobian = 174.26; % Point where the Jacobian is computed => Center of rotation [mm]
+
+ %% Bottom Plate
+ BP = struct();
+
+ BP.rad.int = 0; % Internal Radius [mm]
+ BP.rad.ext = 150; % External Radius [mm]
+ BP.thickness = 10; % Thickness [mm]
+ BP.leg.rad = 100; % Radius where the legs articulations are positionned [mm]
+ BP.leg.ang = 5; % Angle Offset [deg]
+ BP.density = 8000;% Density of the material [kg/m^3]
+ BP.color = [0.7 0.7 0.7]; % Color [rgb]
+ BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
+
+ %% Top Plate
+ TP = struct();
+
+ TP.rad.int = 0; % Internal Radius [mm]
+ TP.rad.ext = 100; % Internal Radius [mm]
+ TP.thickness = 10; % Thickness [mm]
+ TP.leg.rad = 90; % Radius where the legs articulations are positionned [mm]
+ TP.leg.ang = 5; % Angle Offset [deg]
+ TP.density = 8000;% Density of the material [kg/m^3]
+ TP.color = [0.7 0.7 0.7]; % Color [rgb]
+ TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
+
+ %% Leg
+ Leg = struct();
+
+ Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
+ if strcmp(opts.actuator, 'piezo')
+ Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
+ elseif strcmp(opts.actuator, 'lorentz')
+ Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
+ else
+ error('opts.actuator should be piezo or lorentz');
+ end
+ Leg.ksi.ax = 10; % Maximum amplification at resonance []
+ Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
+ Leg.rad.top = 10; % Radius of the cylinder of the top part [mm]
+ Leg.density = 8000; % Density of the material [kg/m^3]
+ Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
+ Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
+
+ Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
+ Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
+ Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
+
+ Leg = updateDamping(Leg);
+
+
+ %% Sphere
+ SP = struct();
+
+ SP.height.bottom = 15; % [mm]
+ SP.height.top = 15; % [mm]
+ SP.density.bottom = 8000; % [kg/m^3]
+ SP.density.top = 8000; % [kg/m^3]
+ SP.color.bottom = [0.7 0.7 0.7]; % [rgb]
+ SP.color.top = [0.7 0.7 0.7]; % [rgb]
+ SP.k.ax = 0; % [N*m/deg]
+ SP.ksi.ax = 3;
+
+ SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
+ SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
+ SP.rad.bottom = Leg.sphere.bottom; % [mm]
+ SP.rad.top = Leg.sphere.top; % [mm]
+ SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
+
+ SP = updateDamping(SP);
+
+ %%
+ Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
+ Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
+
+ %%
+ nano_hexapod.BP = BP;
+ nano_hexapod.TP = TP;
+ nano_hexapod.Leg = Leg;
+ nano_hexapod.SP = SP;
+
+ %%
+ nano_hexapod = initializeParameters(nano_hexapod);
+
+ %% Save
+ save('./mat/stages.mat', 'nano_hexapod', '-append');
+
+ %%
+ function [element] = updateDamping(element)
+ field = fieldnames(element.k);
+ for i = 1:length(field)
+ element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
+ end
+ end
+
+ %%
+ function [stewart] = initializeParameters(stewart)
+ %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
+ stewart.pos_base = zeros(6, 3);
+ stewart.pos_top = zeros(6, 3);
+
+ alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
+ alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
+
+ height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
+
+ radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
+ radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
+
+ for i = 1:3
+ % base points
+ angle_m_b = (2*pi/3)* (i-1) - alpha_b;
+ angle_p_b = (2*pi/3)* (i-1) + alpha_b;
+ stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
+ stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
+
+ % top points
+ % Top points are 60 degrees offset
+ angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
+ angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
+ stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
+ stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
+ end
+
+ % permute pos_top points so that legs are end points of base and top points
+ stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
+ stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
+
+ %% leg vectors
+ legs = stewart.pos_top - stewart.pos_base;
+ leg_length = zeros(6, 1);
+ leg_vectors = zeros(6, 3);
+ for i = 1:6
+ leg_length(i) = norm(legs(i,:));
+ leg_vectors(i,:) = legs(i,:) / leg_length(i);
+ end
+
+ stewart.Leg.lenght = 1000*leg_length(1)/1.5;
+ stewart.Leg.shape.bot = [0 0; ...
+ stewart.Leg.rad.bottom 0; ...
+ stewart.Leg.rad.bottom stewart.Leg.lenght; ...
+ stewart.Leg.rad.top stewart.Leg.lenght; ...
+ stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
+ 0 0.2*stewart.Leg.lenght];
+
+ %% Calculate revolute and cylindrical axes
+ rev1 = zeros(6, 3);
+ rev2 = zeros(6, 3);
+ cyl1 = zeros(6, 3);
+ for i = 1:6
+ rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
+ rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
+
+ rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
+ rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
+
+ cyl1(i,:) = leg_vectors(i,:);
+ end
+
+
+ %% Coordinate systems
+ stewart.lower_leg = struct('rotation', eye(3));
+ stewart.upper_leg = struct('rotation', eye(3));
+
+ for i = 1:6
+ stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+ stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
+ end
+
+ %% Position Matrix
+ stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
+
+ %% Compute Jacobian Matrix
+ aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
+ stewart.J = getJacobianMatrix(leg_vectors', aa');
+ end
+
+ function J = getJacobianMatrix(RM,M_pos_base)
+ % RM: [3x6] unit vector of each leg in the fixed frame
+ % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
+ J = zeros(6);
+ J(:, 1:3) = RM';
+ J(:, 4:6) = cross(M_pos_base, RM)';
+ end
+ end
+#+end_src
+
+** Sample
+ :PROPERTIES:
+ :header-args:matlab+: :tangle src/initializeSample.m
+ :header-args:matlab+: :comments org :mkdirp yes
+ :header-args:matlab+: :eval no :results none
+ :END:
+ <>
+
+This Matlab function is accessible [[file:src/initializeSample.m][here]].
+
+#+begin_src matlab
+ function [sample] = initializeSample(opts_param)
+ %% Default values for opts
+ sample = struct('radius', 100, ...
+ 'height', 300, ...
+ 'mass', 50, ...
+ 'offset', 0, ...
+ 'color', [0.45, 0.45, 0.45] ...
+ );
+
+ %% Populate opts with input parameters
+ if exist('opts_param','var')
+ for opt = fieldnames(opts_param)'
+ sample.(opt{1}) = opts_param.(opt{1});
+ end
+ end
+
+ %%
+ sample.k.x = 1e8;
+ sample.c.x = sqrt(sample.k.x*sample.mass)/10;
+
+ sample.k.y = 1e8;
+ sample.c.y = sqrt(sample.k.y*sample.mass)/10;
+
+ sample.k.z = 1e8;
+ sample.c.z = sqrt(sample.k.y*sample.mass)/10;
+
+ %% Save
+ save('./mat/stages.mat', 'sample', '-append');
+ end
+#+end_src
diff --git a/initialize/initializeGround.m b/initialize/initializeGround.m
deleted file mode 100644
index 3a4e774..0000000
--- a/initialize/initializeGround.m
+++ /dev/null
@@ -1,11 +0,0 @@
-function [ground] = initializeGround()
- %%
- ground = struct();
-
- ground.shape = [2, 2, 0.5]; % [m]
- ground.density = 2800; % [kg/m3]
- ground.color = [0.5, 0.5, 0.5];
-
- %% Save
- save('./mat/stages.mat', 'ground', '-append');
-end
diff --git a/demonstration/demonstration_main.m b/kinematics/demonstration_main.m
similarity index 100%
rename from demonstration/demonstration_main.m
rename to kinematics/demonstration_main.m
diff --git a/demonstration/displacement_init.m b/kinematics/displacement_init.m
similarity index 100%
rename from demonstration/displacement_init.m
rename to kinematics/displacement_init.m
diff --git a/demonstration/displacement_sim.m b/kinematics/displacement_sim.m
similarity index 100%
rename from demonstration/displacement_sim.m
rename to kinematics/displacement_sim.m
diff --git a/demonstration/error_NASS.m b/kinematics/error_NASS.m
similarity index 100%
rename from demonstration/error_NASS.m
rename to kinematics/error_NASS.m
diff --git a/kinematics/index.org b/kinematics/index.org
new file mode 100644
index 0000000..8f1bdda
--- /dev/null
+++ b/kinematics/index.org
@@ -0,0 +1,68 @@
+#+TITLE: Kinematics of the station
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ../index.html
+#+HTML_LINK_UP: ../index.html
+
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+
+#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+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 matlab/modal_frf_coh.m
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell :eval no-export
+
+#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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:
+
+* Introduction :ignore:
+
+* ZIP file containing the data and matlab files :ignore:
+#+begin_src bash :exports none :results none
+ if [ matlab/kinematics.m -nt data/kinematics.zip ]; then
+ cp matlab/kinematics.m kinematics.m;
+ zip data/kinematics \
+ mat/data.mat \
+ kinematics.m
+ rm kinematics.m;
+ fi
+#+end_src
+
+#+begin_note
+ All the files (data and Matlab scripts) are accessible [[file:data/kinematics.zip][here]].
+#+end_note
+
+* Matlab Init :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+ <>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+ <>
+#+end_src
diff --git a/demonstration/sample_pos_init.m b/kinematics/sample_pos_init.m
similarity index 100%
rename from demonstration/sample_pos_init.m
rename to kinematics/sample_pos_init.m
diff --git a/demonstration/sample_pos_sim.m b/kinematics/sample_pos_sim.m
similarity index 100%
rename from demonstration/sample_pos_sim.m
rename to kinematics/sample_pos_sim.m
diff --git a/demonstration/setpoint_vs_position.m b/kinematics/setpoint_vs_position.m
similarity index 100%
rename from demonstration/setpoint_vs_position.m
rename to kinematics/setpoint_vs_position.m
diff --git a/kinematics/sim_nano_station_disp.slx b/kinematics/sim_nano_station_disp.slx
new file mode 100644
index 0000000..c8e291b
Binary files /dev/null and b/kinematics/sim_nano_station_disp.slx differ
diff --git a/kinematics/test2.m b/kinematics/test2.m
new file mode 100644
index 0000000..6dad2b9
--- /dev/null
+++ b/kinematics/test2.m
@@ -0,0 +1,71 @@
+%% Compute position angle from R and Q
+thetas_R = zeros(length(pos.Time), 3);
+thetas_Q = zeros(length(pos.Time), 3);
+
+for i = 1:length(pos.Time)
+ [thetax, thetay, thetaz] = RM2angle(R.Data(:, :, i));
+ thetas_R(i, 1) = thetax; thetas_R(i, 2) = thetay; thetas_R(i, 3) = thetaz;
+
+ [thetax, thetay, thetaz] = quaternionToEulerAngles(Q.Data(i, :));
+ thetas_Q(i, 1) = thetax; thetas_Q(i, 2) = thetay; thetas_Q(i, 3) = thetaz;
+end
+
+%% Compute setpoint
+setpoint_c = zeros(length(pos.Time), 6);
+
+for i = 1:length(pos.Time)
+ setpoint_c(i, :) = computeSetpoint(ty.Data(i), ry.Data(i), rz.Data(i));
+end
+
+%%
+figure;
+hold on;
+plot(pos.Time, pos.Data(:, 1), 'k-', 'DisplayName', 'position');
+plot(pos.Time, setpoint_c(:, 1), '--', 'DisplayName', 'Computed setpoint');
+hold off;
+legend();
+xlabel('Time (s)'); ylabel('Translation (m)');
+
+%%
+figure;
+hold on;
+plot(pos.Time, pos.Data(:, 2), 'k-', 'DisplayName', 'position');
+plot(pos.Time, setpoint_c(:, 2), '--', 'DisplayName', 'Computed setpoint');
+hold off;
+legend();
+xlabel('Time (s)'); ylabel('Translation (m)');
+%%
+figure;
+hold on;
+plot(pos.Time, pos.Data(:, 3), 'k-', 'DisplayName', 'position');
+plot(pos.Time, setpoint_c(:, 3), '--', 'DisplayName', 'Computed setpoint');
+hold off;
+legend();
+xlabel('Time (s)'); ylabel('Translation (m)');
+
+%%
+figure;
+hold on;
+plot(pos.Time, pos.Data(:, 4), 'k-', 'DisplayName', 'position');
+plot(pos.Time, setpoint_c(:, 4), '--', 'DisplayName', 'Computed setpoint');
+hold off;
+legend();
+xlabel('Time (s)'); ylabel('Rotation (rad)');
+
+%%
+figure;
+hold on;
+plot(pos.Time, pos.Data(:, 5), 'k-', 'DisplayName', 'position');
+plot(pos.Time, setpoint_c(:, 5), '--', 'DisplayName', 'Computed setpoint');
+hold off;
+legend();
+xlabel('Time (s)'); ylabel('Rotation (rad)');
+
+%%
+figure;
+hold on;
+plot(pos.Time, pos.Data(:, 6), 'k-', 'DisplayName', 'position');
+plot(pos.Time, setpoint_c(:, 6), '--', 'DisplayName', 'Computed setpoint');
+hold off;
+legend();
+xlabel('Time (s)'); ylabel('Rotation (rad)');
\ No newline at end of file
diff --git a/main.m b/main.m
deleted file mode 100644
index db30065..0000000
--- a/main.m
+++ /dev/null
@@ -1,27 +0,0 @@
-%%
-clear; close all; clc;
-
-%% Open the project
-simulinkproject('./');
-
-%% Initialization
-% Initialize the perturbations
-run init_perturbations.m
-
-% Initialize all the stages parameters
-run init_data.m
-
-%% Demonstration of displacement of all the stages
-run demonstration_main.m
-
-%% Identification
-open id_main.m
-
-%% Active Damping Control
-open act_damp_main.m
-
-%% Control With the Undamped System
-open control_main.m
-
-%% HAC-LAC Control
-open hac_lac_main.m
\ No newline at end of file
diff --git a/nass_library/images/axisc.PNG b/nass_library/images/axisc.PNG
deleted file mode 100644
index 3df59c9..0000000
Binary files a/nass_library/images/axisc.PNG and /dev/null differ
diff --git a/nass_library/images/granite.PNG b/nass_library/images/granite.PNG
deleted file mode 100644
index 6d0fbf3..0000000
Binary files a/nass_library/images/granite.PNG and /dev/null differ
diff --git a/nass_library/images/ground.PNG b/nass_library/images/ground.PNG
deleted file mode 100644
index 05d835c..0000000
Binary files a/nass_library/images/ground.PNG and /dev/null differ
diff --git a/nass_library/images/mirror.PNG b/nass_library/images/mirror.PNG
deleted file mode 100644
index e173eca..0000000
Binary files a/nass_library/images/mirror.PNG and /dev/null differ
diff --git a/nass_library/images/n_hexa.PNG b/nass_library/images/n_hexa.PNG
deleted file mode 100644
index 78c50a5..0000000
Binary files a/nass_library/images/n_hexa.PNG and /dev/null differ
diff --git a/nass_library/images/ry.PNG b/nass_library/images/ry.PNG
deleted file mode 100644
index 60d08e1..0000000
Binary files a/nass_library/images/ry.PNG and /dev/null differ
diff --git a/nass_library/images/rz.PNG b/nass_library/images/rz.PNG
deleted file mode 100644
index ab011ea..0000000
Binary files a/nass_library/images/rz.PNG and /dev/null differ
diff --git a/nass_library/images/sample.PNG b/nass_library/images/sample.PNG
deleted file mode 100644
index cff8124..0000000
Binary files a/nass_library/images/sample.PNG and /dev/null differ
diff --git a/nass_library/images/ty.PNG b/nass_library/images/ty.PNG
deleted file mode 100644
index 71d6421..0000000
Binary files a/nass_library/images/ty.PNG and /dev/null differ
diff --git a/nass_library/images/u_hexa.PNG b/nass_library/images/u_hexa.PNG
deleted file mode 100644
index 147fd3d..0000000
Binary files a/nass_library/images/u_hexa.PNG and /dev/null differ
diff --git a/nass_simscape.org b/nass_simscape.org
deleted file mode 100644
index 09b7281..0000000
--- a/nass_simscape.org
+++ /dev/null
@@ -1,85 +0,0 @@
-#+TITLE: Simscape model - Report
-#+LATEX_CLASS: cleanreport
-#+LaTeX_CLASS_OPTIONS: [tocnp, secbreak]
-#+STARTUP: overview
-#+DATE: 11-2018
-#+LaTeX_HEADER: \newcommand{\authorFirstName}{Thomas}
-#+LaTeX_HEADER: \newcommand{\authorLastName}{Dehaeze}
-#+LaTeX_HEADER: \newcommand{\authorEmail}{dehaeze.thomas@gmail.com}
-#+LaTeX_HEADER: \input{config.tex}
-
-* Simscape files for identification
-|------------------------+----+----+----+------+------|
-| Simscape Name | Ty | Ry | Rz | Hexa | NASS |
-|------------------------+----+----+----+------+------|
-| id micro station | F | F | F | F | |
-| id nano station stages | F | F | F | F | F |
-| id nano station config | D | D | D | D | F |
-| control nano station | D | D | D | D | F |
-|------------------------+----+----+----+------+------|
-
-* Inputs
-** Perturbations
-|----------+--------------------------------------+------+------|
-| Variable | Meaning | Size | Unit |
-|----------+--------------------------------------+------+------|
-| ~Dw~ | Ground motion | 3 | [m] |
-| ~Fg~ | External force applied on granite | 3 | [N] |
-| ~Fs~ | External force applied on the Sample | 3 | [N] |
-|----------+--------------------------------------+------+------|
-
-** Measurement Noise
-|----------+---------+------+------|
-| Variable | Meaning | Size | Unit |
-|----------+---------+------+------|
-| | | | |
-|----------+---------+------+------|
-
-** Control Inputs
-|----------+-------------------------------------------+------+----------|
-| Variable | Meaning | Size | Unit |
-|----------+-------------------------------------------+------+----------|
-| ~Fy~ | Actuation force for Ty | 1 | [N] |
-| ~Dy~ | Imposed displacement for Ty | 1 | [m] |
-|----------+-------------------------------------------+------+----------|
-| ~My~ | Actuation torque for Ry | 1 | [N.m] |
-| ~Ry~ | Imposed rotation for Ry | 1 | [rad] |
-|----------+-------------------------------------------+------+----------|
-| ~Mz~ | Actuation torque for Rz | 1 | [N.m] |
-| ~Rz~ | Imposed rotation for Rz | 1 | [rad] |
-|----------+-------------------------------------------+------+----------|
-| ~Fh~ | Actuation force/torque for hexapod (cart) | 6 | [N, N.m] |
-| ~Fhl~ | Actuation force/torque for hexapod (legs) | 6 | [N] |
-| ~Dh~ | Imposed position for hexapod (cart) | 6 | [m, rad] |
-|----------+-------------------------------------------+------+----------|
-| ~Rm~ | Position of the two masses | 2 | [rad] |
-|----------+-------------------------------------------+------+----------|
-| ~Fn~ | Actuation force for the NASS (cart) | 6 | [N, N.m] |
-| ~Fnl~ | Actuation force for the NASS's legs | 6 | [N] |
-| ~Dn~ | Imposed position for the NASS (cart) | 6 | [m, rad] |
-|----------+-------------------------------------------+------+----------|
-
-* Outputs
-
-|----------+---------------------------------------------+------+--------------|
-| Variable | Meaning | Size | Unit |
-|----------+---------------------------------------------+------+--------------|
-| ~Dgm~ | Absolute displacement of the granite | 3 | [m] |
-| ~Vgm~ | Absolute Velocity of the granite | 3 | [m/s] |
-|----------+---------------------------------------------+------+--------------|
-| ~Dym~ | Measured displacement of Ty | 1 | [m] |
-|----------+---------------------------------------------+------+--------------|
-| ~Rym~ | Measured rotation of Ry | 1 | [rad] |
-|----------+---------------------------------------------+------+--------------|
-| ~Rzm~ | Measured rotation of Rz | 1 | [rad] |
-|----------+---------------------------------------------+------+--------------|
-| ~Dhm~ | Measured position of hexapod (cart) | 6 | [m, rad] |
-|----------+---------------------------------------------+------+--------------|
-| ~Fnlm~ | Measured force of NASS's legs | 6 | [N] |
-| ~Dnlm~ | Measured elongation of NASS's legs | 6 | [m] |
-| ~Dnm~ | Measured position of NASS w.r.t NASS's base | 6 | [m, rad] |
-| ~Vnm~ | Measured absolute velocity of NASS platform | 6 | [m/s, rad/s] |
-| ~Vnlm~ | Measured absolute velocity of NASS's legs | 6 | [m/s] |
-|----------+---------------------------------------------+------+--------------|
-| ~Dsm~ | Position of Sample w.r.t. granite frame | 6 | [m, rad] |
-|----------+---------------------------------------------+------+--------------|
diff --git a/project_shutdown.m b/project_shutdown.m
deleted file mode 100644
index d7948f6..0000000
--- a/project_shutdown.m
+++ /dev/null
@@ -1 +0,0 @@
-Simulink.fileGenControl('reset');
diff --git a/run_simulations.m b/run_simulations.m
deleted file mode 100644
index bccd69b..0000000
--- a/run_simulations.m
+++ /dev/null
@@ -1,41 +0,0 @@
-%% Open Loop simulation and save the final state
-steady_time = 10;
-
-initializeSimConf(struct('Tsim', steady_time, 'cl_time', steady_time));
-
-set_param('sim_nano_station_ctrl',...
- 'SaveFinalState','on',...
- 'FinalStateName','myOperPoint',...
- 'SaveCompleteFinalSimState','on'...
-);
-
-sim('sim_nano_station_ctrl');
-
-save('./data/myOperPoint.mat', 'myOperPoint');
-
-set_param('sim_nano_station_ctrl',...
- 'SaveFinalState','off',...
- 'SaveCompleteFinalSimState','off'...
-);
-
-save('./data/exp_open_loop.mat', 'Dmeas');
-
-%% Close the Loop and start from steady state
-sim_time = 10;
-
-initializeSimConf(struct('Tsim', steady_time+sim_time, 'cl_time', steady_time));
-
-load('./data/myOperPoint.mat', 'myOperPoint');
-
-set_param('sim_nano_station_ctrl',...
- 'LoadInitialState','on',...
- 'InitialState','myOperPoint'...
-);
-
-sim('sim_nano_station_ctrl');
-
-set_param('sim_nano_station_ctrl',...
- 'LoadInitialState','off' ...
-);
-
-save('./data/exp_close_loop_xyz.mat', 'Dmeas');
diff --git a/sim_nano_station_ctrl.slx b/sim_nano_station_ctrl.slx
index 36c3181..87cb382 100644
Binary files a/sim_nano_station_ctrl.slx and b/sim_nano_station_ctrl.slx differ
diff --git a/slprj/grt/Assemblage/tmwinternal/minfo.mat b/slprj/grt/Assemblage/tmwinternal/minfo.mat
deleted file mode 100644
index 3b1e5c3..0000000
Binary files a/slprj/grt/Assemblage/tmwinternal/minfo.mat and /dev/null differ
diff --git a/slprj/sl_proj.tmw b/slprj/sl_proj.tmw
deleted file mode 100644
index 1d13cf1..0000000
--- a/slprj/sl_proj.tmw
+++ /dev/null
@@ -1,2 +0,0 @@
-Simulink Coder project marker file. Please don't change it.
-slprjVersion: 9.0_037
\ No newline at end of file
diff --git a/src/computePsdDispl.m b/src/computePsdDispl.m
index 0e41ca5..aa97514 100644
--- a/src/computePsdDispl.m
+++ b/src/computePsdDispl.m
@@ -1,3 +1,14 @@
+% computePsdDispl
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/computePsdDispl.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/computePsdDispl.m][here]].
+
+
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
i_init = find(sys_data.time > t_init, 1);
diff --git a/src/computeSetpoint.m b/src/computeSetpoint.m
new file mode 100644
index 0000000..df69a3d
--- /dev/null
+++ b/src/computeSetpoint.m
@@ -0,0 +1,70 @@
+% computeSetpoint
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/computeSetpoint.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/computeSetpoint.m][here]].
+
+
+function setpoint = computeSetpoint(ty, ry, rz)
+%%
+setpoint = zeros(6, 1);
+
+%% Ty
+Ty = [1 0 0 0 ;
+ 0 1 0 ty ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+% Tyinv = [1 0 0 0 ;
+% 0 1 0 -ty ;
+% 0 0 1 0 ;
+% 0 0 0 1 ];
+
+%% Ry
+Ry = [ cos(ry) 0 sin(ry) 0 ;
+ 0 1 0 0 ;
+ -sin(ry) 0 cos(ry) 0 ;
+ 0 0 0 1 ];
+
+% TMry = Ty*Ry*Tyinv;
+
+%% Rz
+Rz = [cos(rz) -sin(rz) 0 0 ;
+ sin(rz) cos(rz) 0 0 ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+% TMrz = Ty*TMry*Rz*TMry'*Tyinv;
+
+%% All stages
+% TM = TMrz*TMry*Ty;
+
+TM = Ty*Ry*Rz;
+
+[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
+
+setpoint(1:3) = TM(1:3, 4);
+setpoint(4:6) = [thetax, thetay, thetaz];
+
+%% Custom Functions
+function [thetax, thetay, thetaz] = RM2angle(R)
+ if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+ thetay = -asin(R(3, 1));
+ thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+ thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+ else
+ thetaz = 0;
+ if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+ thetay = pi/2;
+ thetax = thetaz + atan2(R(1, 2), R(1, 3));
+ else
+ thetay = -pi/2;
+ thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+ end
+ end
+end
+end
diff --git a/src/converErrorBasis.m b/src/converErrorBasis.m
new file mode 100644
index 0000000..3a33109
--- /dev/null
+++ b/src/converErrorBasis.m
@@ -0,0 +1,136 @@
+% converErrorBasis
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/converErrorBasis.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/converErrorBasis.m][here]].
+
+
+function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
+% convertErrorBasis -
+%
+% Syntax: convertErrorBasis(p_error, ty, ry, rz)
+%
+% Inputs:
+% - p_error - Position error of the sample w.r.t. the granite [m, rad]
+% - ty - Measured translation of the Ty stage [m]
+% - ry - Measured rotation of the Ry stage [rad]
+% - rz - Measured rotation of the Rz stage [rad]
+%
+% Outputs:
+% - P_nass - Position error of the sample w.r.t. the NASS base [m]
+% - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
+%
+% Example:
+%
+
+%% If line vector => column vector
+if size(pos, 2) == 6
+ pos = pos';
+end
+
+if size(setpoint, 2) == 6
+ setpoint = setpoint';
+end
+
+%% Position of the sample in the frame fixed to the Granite
+P_granite = [pos(1:3); 1]; % Position [m]
+R_granite = [setpoint(1:3); 1]; % Reference [m]
+
+%% Transformation matrices of the stages
+% T-y
+TMty = [1 0 0 0 ;
+ 0 1 0 ty ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+% R-y
+TMry = [ cos(ry) 0 sin(ry) 0 ;
+ 0 1 0 0 ;
+ -sin(ry) 0 cos(ry) 0 ;
+ 0 0 0 1 ];
+
+% R-z
+TMrz = [cos(rz) -sin(rz) 0 0 ;
+ sin(rz) cos(rz) 0 0 ;
+ 0 0 1 0 ;
+ 0 0 0 1 ];
+
+%% Compute Point coordinates in the new reference fixed to the NASS base
+% P_nass = TMrz*TMry*TMty*P_granite;
+P_nass = TMrz\TMry\TMty\P_granite;
+R_nass = TMrz\TMry\TMty\R_granite;
+
+dx = R_nass(1)-P_nass(1);
+dy = R_nass(2)-P_nass(2);
+dz = R_nass(3)-P_nass(3);
+
+%% Compute new basis vectors linked to the NASS base
+% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
+% ux_nass = ux_nass(1:3);
+% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
+% uy_nass = uy_nass(1:3);
+% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
+% uz_nass = uz_nass(1:3);
+
+ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
+ux_nass = ux_nass(1:3);
+uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
+uy_nass = uy_nass(1:3);
+uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
+uz_nass = uz_nass(1:3);
+
+%% Rotations error w.r.t. granite Frame
+% Rotations error w.r.t. granite Frame
+rx_nass = pos(4);
+ry_nass = pos(5);
+rz_nass = pos(6);
+
+% Rotation matrices of the Sample w.r.t. the Granite
+Mrx_error = [1 0 0 ;
+ 0 cos(-rx_nass) -sin(-rx_nass) ;
+ 0 sin(-rx_nass) cos(-rx_nass)];
+
+Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
+ 0 1 0 ;
+ -sin(-ry_nass) 0 cos(-ry_nass)];
+
+Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
+ sin(-rz_nass) cos(-rz_nass) 0 ;
+ 0 0 1];
+
+% Rotation matrix of the Sample w.r.t. the Granite
+Mr_error = Mrz_error*Mry_error*Mrx_error;
+
+%% Use matrix to solve
+R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample
+
+[thetax, thetay, thetaz] = RM2angle(R);
+
+error_nass = [dx; dy; dz; thetax; thetay; thetaz];
+
+%% Custom Functions
+function [thetax, thetay, thetaz] = RM2angle(R)
+ if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+ thetay = -asin(R(3, 1));
+ % thetaybis = pi-thetay;
+ thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+ % thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
+ thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+ % thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
+ else
+ thetaz = 0;
+ if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+ thetay = pi/2;
+ thetax = thetaz + atan2(R(1, 2), R(1, 3));
+ else
+ thetay = -pi/2;
+ thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+ end
+ end
+end
+
+end
diff --git a/src/generateDiagPidControl.m b/src/generateDiagPidControl.m
index 47a777a..406eea8 100644
--- a/src/generateDiagPidControl.m
+++ b/src/generateDiagPidControl.m
@@ -1,3 +1,14 @@
+% generateDiagPidControl
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/generateDiagPidControl.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/generateDiagPidControl.m][here]].
+
+
function [K] = generateDiagPidControl(G, fs)
%%
pid_opts = pidtuneOptions(...
diff --git a/src/identifyPlant.m b/src/identifyPlant.m
index 640acc4..1743247 100644
--- a/src/identifyPlant.m
+++ b/src/identifyPlant.m
@@ -1,3 +1,14 @@
+% identifyPlant
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/identifyPlant.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/identifyPlant.m][here]].
+
+
function [sys] = identifyPlant(opts_param)
%% Default values for opts
opts = struct();
diff --git a/src/init_simulation.m b/src/init_simulation.m
new file mode 100644
index 0000000..64a1709
--- /dev/null
+++ b/src/init_simulation.m
@@ -0,0 +1,19 @@
+% Simulation Initialization
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/init_simulation.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab script is accessible [[file:src/init_simulation.m][here]].
+
+% This script runs just before the simulation is started.
+% It is used to load the simulation configuration and the controllers used for the simulation.
+
+
+%% Load all the data used for the simulation
+load('./mat/sim_conf.mat');
+
+%% Load Controller
+load('./mat/controllers.mat');
diff --git a/initialize/initializeAxisc.m b/src/initializeAxisc.m
similarity index 76%
rename from initialize/initializeAxisc.m
rename to src/initializeAxisc.m
index f2282ed..98a7e6d 100644
--- a/initialize/initializeAxisc.m
+++ b/src/initializeAxisc.m
@@ -1,3 +1,14 @@
+% Center of gravity compensation
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeAxisc.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeAxisc.m][here]].
+
+
function [axisc] = initializeAxisc()
%%
axisc = struct();
diff --git a/initialize/initializeExperiment.m b/src/initializeExperiment.m
similarity index 70%
rename from initialize/initializeExperiment.m
rename to src/initializeExperiment.m
index 4095832..aa9c7f1 100644
--- a/initialize/initializeExperiment.m
+++ b/src/initializeExperiment.m
@@ -1,3 +1,14 @@
+% Experiment
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeExperiment.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeExperiment.m][here]].
+
+
function [] = initializeExperiment(exp_name, sys_mass)
if strcmp(exp_name, 'tomography')
opts_sim = struct(...
diff --git a/initialize/initializeGranite.m b/src/initializeGranite.m
similarity index 70%
rename from initialize/initializeGranite.m
rename to src/initializeGranite.m
index d47b17c..4551d64 100644
--- a/initialize/initializeGranite.m
+++ b/src/initializeGranite.m
@@ -1,3 +1,14 @@
+% Granite
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeGranite.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeGranite.m][here]].
+
+
function [granite] = initializeGranite()
%%
granite = struct();
diff --git a/src/initializeGround.m b/src/initializeGround.m
new file mode 100644
index 0000000..101e2da
--- /dev/null
+++ b/src/initializeGround.m
@@ -0,0 +1,22 @@
+% Ground
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeGround.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeGround.m][here]].
+
+
+function [ground] = initializeGround()
+ %%
+ ground = struct();
+
+ ground.shape = [2, 2, 0.5]; % [m]
+ ground.density = 2800; % [kg/m3]
+ ground.color = [0.5, 0.5, 0.5];
+
+ %% Save
+ save('./mat/stages.mat', 'ground', '-append');
+end
diff --git a/initialize/initializeInputs.m b/src/initializeInputs.m
similarity index 93%
rename from initialize/initializeInputs.m
rename to src/initializeInputs.m
index 66e50e7..c3df5cc 100644
--- a/initialize/initializeInputs.m
+++ b/src/initializeInputs.m
@@ -1,3 +1,14 @@
+% Inputs
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeInputs.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeInputs.m][here]].
+
+
function [inputs] = initializeInputs(opts_param)
%% Default values for opts
opts = struct( ...
@@ -136,7 +147,7 @@ function [inputs] = initializeInputs(opts_param)
%% Ry
TMRy = [ cos(ry) 0 sin(ry) 0 ;
- 0 1 0 0 ;
+ 0 1 0 0 ;
-sin(ry) 0 cos(ry) 0 ;
0 0 0 1 ];
diff --git a/initialize/initializeMicroHexapod.m b/src/initializeMicroHexapod.m
similarity index 90%
rename from initialize/initializeMicroHexapod.m
rename to src/initializeMicroHexapod.m
index c6063d9..3cca317 100644
--- a/initialize/initializeMicroHexapod.m
+++ b/src/initializeMicroHexapod.m
@@ -1,3 +1,14 @@
+% Micro Hexapod
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeMicroHexapod.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeMicroHexapod.m][here]].
+
+
function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Default values for opts
opts = struct();
@@ -107,7 +118,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
stewart.pos_base = zeros(6, 3);
stewart.pos_top = zeros(6, 3);
- alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
+ alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
@@ -145,11 +156,11 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
stewart.Leg.shape.bot = [0 0; ...
- stewart.Leg.rad.bottom 0; ...
- stewart.Leg.rad.bottom stewart.Leg.lenght; ...
- stewart.Leg.rad.top stewart.Leg.lenght; ...
- stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
- 0 0.2*stewart.Leg.lenght];
+ stewart.Leg.rad.bottom 0; ...
+ stewart.Leg.rad.bottom stewart.Leg.lenght; ...
+ stewart.Leg.rad.top stewart.Leg.lenght; ...
+ stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
+ 0 0.2*stewart.Leg.lenght];
%% Calculate revolute and cylindrical axes
rev1 = zeros(6, 3);
diff --git a/initialize/initializeMirror.m b/src/initializeMirror.m
similarity index 86%
rename from initialize/initializeMirror.m
rename to src/initializeMirror.m
index 10234df..a4d946e 100644
--- a/initialize/initializeMirror.m
+++ b/src/initializeMirror.m
@@ -1,3 +1,14 @@
+% Mirror
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeMirror.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeMirror.m][here]].
+
+
function [] = initializeMirror(opts_param)
%% Default values for opts
opts = struct(...
diff --git a/initialize/initializeNanoHexapod.m b/src/initializeNanoHexapod.m
similarity index 90%
rename from initialize/initializeNanoHexapod.m
rename to src/initializeNanoHexapod.m
index 19b084f..f8ee13c 100644
--- a/initialize/initializeNanoHexapod.m
+++ b/src/initializeNanoHexapod.m
@@ -1,3 +1,14 @@
+% Nano Hexapod
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeNanoHexapod.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeNanoHexapod.m][here]].
+
+
function [nano_hexapod] = initializeNanoHexapod(opts_param)
%% Default values for opts
opts = struct('actuator', 'piezo');
@@ -114,7 +125,7 @@ function [nano_hexapod] = initializeNanoHexapod(opts_param)
stewart.pos_base = zeros(6, 3);
stewart.pos_top = zeros(6, 3);
- alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
+ alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
@@ -152,11 +163,11 @@ function [nano_hexapod] = initializeNanoHexapod(opts_param)
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
stewart.Leg.shape.bot = [0 0; ...
- stewart.Leg.rad.bottom 0; ...
- stewart.Leg.rad.bottom stewart.Leg.lenght; ...
- stewart.Leg.rad.top stewart.Leg.lenght; ...
- stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
- 0 0.2*stewart.Leg.lenght];
+ stewart.Leg.rad.bottom 0; ...
+ stewart.Leg.rad.bottom stewart.Leg.lenght; ...
+ stewart.Leg.rad.top stewart.Leg.lenght; ...
+ stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
+ 0 0.2*stewart.Leg.lenght];
%% Calculate revolute and cylindrical axes
rev1 = zeros(6, 3);
diff --git a/initialize/initializeRy.m b/src/initializeRy.m
similarity index 86%
rename from initialize/initializeRy.m
rename to src/initializeRy.m
index 71afabf..5800edb 100644
--- a/initialize/initializeRy.m
+++ b/src/initializeRy.m
@@ -1,3 +1,14 @@
+% Tilt Stage
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeRy.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeRy.m][here]].
+
+
function [ry] = initializeRy(opts_param)
%% Default values for opts
opts = struct('rigid', false);
@@ -47,7 +58,7 @@ function [ry] = initializeRy(opts_param)
ry.c.rad = 10*(1/5)*sqrt(ry.k.rad/ry.m);
ry.c.rrad = 10*(1/5)*sqrt(ry.k.rrad/ry.m);
ry.c.tilt = 10*(1/1)*sqrt(ry.k.tilt/ry.m);
-
+
%% Positioning parameters
ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m]
diff --git a/initialize/initializeRz.m b/src/initializeRz.m
similarity index 84%
rename from initialize/initializeRz.m
rename to src/initializeRz.m
index 18164bd..849612f 100644
--- a/initialize/initializeRz.m
+++ b/src/initializeRz.m
@@ -1,3 +1,14 @@
+% Spindle
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeRz.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeRz.m][here]].
+
+
function [rz] = initializeRz(opts_param)
%% Default values for opts
opts = struct('rigid', false);
diff --git a/initialize/initializeSample.m b/src/initializeSample.m
similarity index 71%
rename from initialize/initializeSample.m
rename to src/initializeSample.m
index deff75b..b7a824d 100644
--- a/initialize/initializeSample.m
+++ b/src/initializeSample.m
@@ -1,3 +1,14 @@
+% Sample
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeSample.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeSample.m][here]].
+
+
function [sample] = initializeSample(opts_param)
%% Default values for opts
sample = struct('radius', 100, ...
@@ -13,7 +24,7 @@ function [sample] = initializeSample(opts_param)
sample.(opt{1}) = opts_param.(opt{1});
end
end
-
+
%%
sample.k.x = 1e8;
sample.c.x = sqrt(sample.k.x*sample.mass)/10;
diff --git a/initialize/initializeSimConf.m b/src/initializeSimConf.m
similarity index 72%
rename from initialize/initializeSimConf.m
rename to src/initializeSimConf.m
index 8cbd1c2..865c531 100644
--- a/initialize/initializeSimConf.m
+++ b/src/initializeSimConf.m
@@ -1,3 +1,14 @@
+% Simulation Configuration
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeSimConf.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeSimConf.m][here]].
+
+
function [] = initializeSimConf(opts_param)
%% Default values for opts
opts = struct('Ts', 1e-4, ... % Sampling time [s]
diff --git a/initialize/initializeTy.m b/src/initializeTy.m
similarity index 89%
rename from initialize/initializeTy.m
rename to src/initializeTy.m
index 43b1d9b..37704d8 100644
--- a/initialize/initializeTy.m
+++ b/src/initializeTy.m
@@ -1,3 +1,14 @@
+% Translation Stage
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/initializeTy.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/initializeTy.m][here]].
+
+
function [ty] = initializeTy(opts_param)
%% Default values for opts
opts = struct('rigid', false);
@@ -8,7 +19,7 @@ function [ty] = initializeTy(opts_param)
opts.(opt{1}) = opts_param.(opt{1});
end
end
-
+
%%
ty = struct();
diff --git a/src/project_shutdown.m b/src/project_shutdown.m
new file mode 100644
index 0000000..5bb8d70
--- /dev/null
+++ b/src/project_shutdown.m
@@ -0,0 +1,5 @@
+
+
+% When the project closes, it runs the =project_shutdown.m= script defined below.
+
+Simulink.fileGenControl('reset');
diff --git a/project_startup.m b/src/project_startup.m
similarity index 74%
rename from project_startup.m
rename to src/project_startup.m
index 276abe4..92f186a 100644
--- a/project_startup.m
+++ b/src/project_startup.m
@@ -1,3 +1,8 @@
+
+
+% When the project opens, a startup script is ran.
+% The startup script is defined below and is exported to the =project_startup.m= script.
+
%%
freqs = logspace(-1, 3, 1000);
save_fig = false;
diff --git a/src/runSimulation.m b/src/runSimulation.m
index 16801b7..9a151ef 100644
--- a/src/runSimulation.m
+++ b/src/runSimulation.m
@@ -1,3 +1,14 @@
+% runSimulation
+% :PROPERTIES:
+% :header-args:matlab+: :tangle src/runSimulation.m
+% :header-args:matlab+: :comments org :mkdirp yes
+% :header-args:matlab+: :eval no :results none
+% :END:
+% <>
+
+% This Matlab function is accessible [[file:src/runSimulation.m][here]].
+
+
function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
%% Load the controller and save it for the simulation
if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
@@ -27,7 +38,7 @@ function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
%%
if strcmp(sys_name, 'pz')
- initializeNanoHexapod(struct('actuator', 'piezo'));
+ initializeNanoHexapod(struct('actuator', 'piezo'));
elseif strcmp(sys_name, 'vc')
initializeNanoHexapod(struct('actuator', 'lorentz'));
else
@@ -35,7 +46,7 @@ function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
end
if strcmp(sys_mass, 'light')
- initializeSample(struct('mass', 1));
+ initializeSample(struct('mass', 1));
elseif strcmp(sys_mass, 'heavy')
initializeSample(struct('mass', 50));
else