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