tdehaeze/phd-simscape-micro-station
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
45bef195d6
phd-simscape-micro-station/simscape-micro-station.org

2316 lines
68 KiB
Org Mode
Raw Blame History

#+TITLE: Simscape Model - Micro Station
:DRAWER:
#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas
#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../index.html
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
#+BIND: org-latex-image-default-option "scale=1"
#+BIND: org-latex-image-default-width ""
#+LaTeX_CLASS: scrreprt
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
#+LATEX_HEADER: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
#+LATEX_HEADER_EXTRA: \bibliography{simscape-micro-station.bib}
#+BIND: org-latex-bib-compiler "biber"
#+PROPERTY: header-args:matlab :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports none
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :tangle no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:
#+begin_export html
<hr>
<p>This report is also available as a <a href="./simscape-micro-station.pdf">pdf</a>.</p>
<hr>
#+end_export
#+latex: \clearpage
* Build :noexport:
#+NAME: startblock
#+BEGIN_SRC emacs-lisp :results none :tangle no
(add-to-list 'org-latex-classes
'("scrreprt"
"\\documentclass{scrreprt}"
("\\chapter{%s}" . "\\chapter*{%s}")
("\\section{%s}" . "\\section*{%s}")
("\\subsection{%s}" . "\\subsection*{%s}")
("\\paragraph{%s}" . "\\paragraph*{%s}")
))
;; Remove automatic org heading labels
(defun my-latex-filter-removeOrgAutoLabels (text backend info)
"Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
(when (org-export-derived-backend-p backend 'latex)
(replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text)))
(add-to-list 'org-export-filter-headline-functions
'my-latex-filter-removeOrgAutoLabels)
;; Remove all org comments in the output LaTeX file
(defun delete-org-comments (backend)
(loop for comment in (reverse (org-element-map (org-element-parse-buffer)
'comment 'identity))
do
(setf (buffer-substring (org-element-property :begin comment)
(org-element-property :end comment))
"")))
(add-hook 'org-export-before-processing-hook 'delete-org-comments)
;; Use no package by default
(setq org-latex-packages-alist nil)
(setq org-latex-default-packages-alist nil)
;; Do not include the subtitle inside the title
(setq org-latex-subtitle-separate t)
(setq org-latex-subtitle-format "\\subtitle{%s}")
(setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing
org-ref-acronyms-before-parsing))
#+END_SRC
* Notes :noexport:
** Notes
Prefix is =ustation=
From modal analysis: validation of the multi-body model.
*Goals*:
- *Modelling of the micro-station*: Kinematics + Dynamics + Disturbances
- Kinematics of each stage
- Modelling: solid bodies + joints. Show what is used for each stage
- Correlation with the dynamical measurements
- Inclusion of disturbances (correlation with measurements)
*Notes*:
- Do not talk about Nano-Hexapod yet
- Do not talk about external metrology
Based on:
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org][kinematics]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/simscape_subsystems.org][simscape_subsystems]]: general presentation of the micro-station. Used model: solid body + joints. Presentation of each stage.
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/documents/work-package-1/work-package-1.org::*Specification of requirements][Specification of requirements]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/identification.org][identification]]: comparison of measurements and simscape model (not so good?)
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/experiments.org][experiments]]: simulation of experiments with the Simscape model
- [ ] Measurement of disturbances / things that will have to be corrected using the NASS:
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-to-dynamic/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-control-system/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-sr-rz/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/ground-motion/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-spindle/index.org]]
- [ ] Check [[file:~/Cloud/work-projects/ID31-NASS/specifications/id-31-spindle-meas][this directory]] and [[file:~/Cloud/work-projects/ID31-NASS/specifications/stage-by-stage][this directory]]
** DONE [#B] Put colors for each different stage
CLOSED: [2024-10-30 Wed 14:07]
** DONE [#B] Delete Gravity compensation Stage
CLOSED: [2024-10-30 Wed 13:51]
** DONE [#B] Maybe make a simpler Simscape model for this report
CLOSED: [2024-10-30 Wed 13:51]
** DONE [#A] Open the Simscape model and verify it all works
CLOSED: [2024-10-30 Wed 13:33] SCHEDULED: <2024-10-30 Wed>
#+begin_src matlab
initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'none');
initializeGround( 'type', 'rigid');
initializeGranite( 'type', 'modal-analysis');
initializeTy( 'type', 'modal-analysis');
initializeRy( 'type', 'modal-analysis');
initializeRz( 'type', 'modal-analysis');
initializeMicroHexapod('type', 'modal-analysis');
initializeReferences();
initializeDisturbances('enable', false);
#+end_src
#+begin_src matlab
% initializeAxisc( 'type', 'flexible');
% initializeMirror( 'type', 'none');
% initializeNanoHexapod( 'type', 'none');
% initializeSample( 'type', 'none');
initializeController( 'type', 'open-loop');
#+end_src
** TODO [#B] Make good "init" for the Simscape model
** TODO [#C] Verify that we get "correct" compliance
* Introduction :ignore:
#+name: tab:ustation_section_matlab_code
#+caption: Report sections and corresponding Matlab files
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
#+attr_latex: :center t :booktabs t
| *Sections* | *Matlab File* |
|---------------------------------------------+-------------------------------------|
| Section ref:sec: | =ustation_1_.m= |
* Micro-Station Kinematics
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/.m
:END:
<<sec:ustation_kinematics>>
** Introduction :ignore:
[[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org]]
# - Small overview of each stage and associated stiffnesses / inertia
# - schematic that shows to considered DoF
# - import from CAD
** Matlab Init :noexport:ignore:
#+begin_src matlab
%% .m
#+end_src
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no :noweb yes
<<m-init-path>>
#+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-simscape>>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src
** Granite
** Translation Stage
** Tilt Stage
** Spindle
** Micro-Hexapod
* Stage Modeling
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/.m
:END:
<<sec:ustation_kinematics>>
** Introduction :ignore:
The goal here is to tune the Simscape model of the station in order to have a good dynamical representation of the real system.
In order to do so, we reproduce the Modal Analysis done on the station using the Simscape model.
We can then compare the measured Frequency Response Functions with the identified dynamics of the model.
Finally, this should help to tune the parameters of the model such that the dynamics is closer to the measured FRF.
# Validation of the Model
# - Most important metric: support compliance
# - Compare model and measurement
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no :noweb yes
<<m-init-path>>
#+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src
** Some notes about the Simscape Model
The Simscape Model of the micro-station consists of several solid bodies:
- Bottom Granite
- Top Granite
- Translation Stage
- Tilt Stage
- Spindle
- Hexapod
Each solid body has some characteristics: Center of Mass, mass, moment of inertia, etc...
These parameters are automatically computed from the geometry and from the density of the materials.
Then, the solid bodies are connected with springs and dampers.
Some of the springs and dampers values can be estimated from the joints/stages specifications, however, we here prefer to tune these values based on the measurements.
** Compare with measurements at the CoM of each element
*** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
*** Prepare the Simulation
We load the configuration.
#+begin_src matlab
load('mat/conf_simulink.mat');
#+end_src
We set a small =StopTime=.
#+begin_src matlab
set_param(conf_simulink, 'StopTime', '0.5');
#+end_src
We initialize all the stages.
#+begin_src matlab
initializeGround( 'type', 'rigid');
initializeGranite( 'type', 'modal-analysis');
initializeTy( 'type', 'modal-analysis');
initializeRy( 'type', 'modal-analysis');
initializeRz( 'type', 'modal-analysis');
initializeMicroHexapod('type', 'modal-analysis');
initializeAxisc( 'type', 'flexible');
initializeMirror( 'type', 'none');
initializeNanoHexapod( 'type', 'none');
initializeSample( 'type', 'none');
initializeController( 'type', 'open-loop');
initializeLoggingConfiguration('log', 'none');
initializeReferences();
initializeDisturbances('enable', false);
#+end_src
*** Estimate the position of the CoM of each solid and compare with the one took for the Measurement Analysis
Thanks to the [[https://fr.mathworks.com/help/physmod/sm/ref/inertiasensor.html][Inertia Sensor]] simscape block, it is possible to estimate the position of the Center of Mass of a solid body with respect to a defined frame.
#+begin_src matlab
sim('nass_model')
#+end_src
The results are shown in the table [[tab:com_simscape]].
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
stages_com = 1e3*[granite_bot_com.Data(end, :) ;
granite_top_com.Data(end, :) ;
ty_com.Data(end, :) ;
ry_com.Data(end, :) ;
rz_com.Data(end, :) ;
hexa_com.Data(end, :) ]';
data2orgtable(stages_com, {'X [mm]', 'Y [mm]', 'Z [mm]'}, {'granite bot', 'granite top', 'ty', 'ry', 'rz', 'hexa'}, ' %.1f ');
#+end_src
#+name: tab:com_simscape
#+caption: Center of Mass of each solid body as defined in Simscape
#+RESULTS:
| | granite bot | granite top | ty | ry | rz | hexa |
|--------+-------------+-------------+--------+--------+--------+--------|
| X [mm] | 52.4 | 51.7 | 0.9 | -0.1 | 0.0 | -0.0 |
| Y [mm] | 190.4 | 263.2 | 0.7 | 5.2 | -0.0 | 0.1 |
| Z [mm] | -1200.0 | -777.1 | -598.9 | -627.7 | -643.2 | -317.1 |
We can compare the obtained center of mass (table [[tab:com_simscape]]) with the one used for the Modal Analysis shown in table [[tab:com_solidworks]].
#+name: tab:com_solidworks
#+caption: Estimated Center of Mass of each solid body using Solidworks
| | granite bot | granite top | ty | ry | rz | hexa |
|--------+-------------+-------------+------+------+------+------|
| X [mm] | 45 | 52 | 0 | 0 | 0 | -4 |
| Y [mm] | 144 | 258 | 14 | -5 | 0 | 6 |
| Z [mm] | -1251 | -778 | -600 | -628 | -580 | -319 |
The results are quite similar.
The differences can be explained by some differences in the chosen density of the materials or by the fact that not exactly all the same elements have been chosen for each stage.
For instance, on simscape, the fixed part of the translation stage counts for the top granite solid body.
However, in SolidWorks, this has probably not be included with the top granite.
*** Create a frame at the CoM of each solid body
Now we use one =inertiasensor= block connected on each solid body that measured the center of mass of this solid with respect to the same connected frame.
We do that in order to position an accelerometer on the Simscape model at this particular point.
#+begin_src matlab
open('identification/matlab/sim_micro_station_com_estimation.slx')
#+end_src
#+begin_src matlab
sim('sim_micro_station_com_estimation')
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
stages_com = 1e3*[granite_bot_com.Data(end, :) ;
granite_top_com.Data(end, :) ;
ty_com.Data(end, :) ;
ry_com.Data(end, :) ;
rz_com.Data(end, :) ;
hexa_com.Data(end, :) ]';
data2orgtable(stages_com, {'X [mm]', 'Y [mm]', 'Z [mm]'}, {'granite bot', 'granite top', 'ty', 'ry', 'rz', 'hexa'}, ' %.1f ');
#+end_src
#+RESULTS:
| | granite bot | granite top | ty | ry | rz | hexa |
|--------+-------------+-------------+-------+--------+-------+-------|
| X [mm] | 0.0 | 51.7 | 0.9 | -0.1 | 0.0 | -0.0 |
| Y [mm] | 0.0 | 753.2 | 0.7 | 5.2 | -0.0 | 0.1 |
| Z [mm] | -250.0 | 22.9 | -17.1 | -146.5 | -23.2 | -47.1 |
We now same this for further use:
#+begin_src matlab
granite_bot_com = granite_bot_com.Data(end, :)';
granite_top_com = granite_top_com.Data(end, :)';
ty_com = ty_com.Data(end, :)';
ry_com = ry_com.Data(end, :)';
rz_com = rz_com.Data(end, :)';
hexa_com = hexa_com.Data(end, :)';
save('./mat/solids_com.mat', 'granite_bot_com', 'granite_top_com', 'ty_com', 'ry_com', 'rz_com', 'hexa_com');
#+end_src
Then, we use the obtained results to add a =rigidTransform= block in order to create a new frame at the center of mass of each solid body.
*** Identification of the dynamics of the Simscape Model
We now use a new Simscape Model where 6DoF inertial sensors are located at the Center of Mass of each solid body.
#+begin_src matlab
% load('mat/solids_com.mat', 'granite_bot_com', 'granite_top_com', 'ty_com', 'ry_com', 'rz_com', 'hexa_com');
#+end_src
#+begin_src matlab
open('nass_model.slx')
#+end_src
We use the =linearize= function in order to estimate the dynamics from forces applied on the Translation stage at the same position used for the real modal analysis to the inertial sensors.
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Granite/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
#+end_src
#+begin_src matlab
% Run the linearization
G_ms = linearize(mdl, io, 0);
%% Input/Output definition
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
#+end_src
The output of =G_ms= is the acceleration of each solid body.
In order to obtain a displacement, we divide the obtained transfer function by $1/s^{2}$;
#+begin_src matlab
G_ms = G_ms/s^2;
#+end_src
*** Compare with measurements
We now load the Frequency Response Functions measurements during the Modal Analysis (accessible [[file:../../meas/modal-analysis/index.org][here]]).
#+begin_src matlab
load('../meas/modal-analysis/mat/frf_coh_matrices.mat', 'freqs');
load('../meas/modal-analysis/mat/frf_com.mat', 'FRFs_CoM');
#+end_src
We then compare the measurements with the identified transfer functions using the Simscape Model.
#+begin_src matlab :exports none
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'}
n_stg = 3;
n_dir = 6; % x, y, z, Rx, Ry, Rz
n_exc = 2; % x, y, z
f = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg-1) + n_dir, n_exc, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
xlim([1, 200]);
#+end_src
#+begin_src matlab :exports none
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'gtop', 'ty', 'ry', 'rz', 'hexa'}
f = logspace(1, 3, 1000);
figure;
for n_stg = 1:2
for n_dir = 1:3
subplot(3, 2, (n_dir-1)*2 + n_stg);
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
if n_dir == 3
xlabel('Frequency [Hz]');
end
hold off;
xlim([10, 1000]);
ylim([1e-12, 1e-6]);
end
end
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_comp_bot_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:identification_comp_bot_stages
#+CAPTION: caption ([[./figs/identification_comp_bot_stages.png][png]], [[./figs/identification_comp_bot_stages.pdf][pdf]])
[[file:figs/identification_comp_bot_stages.png]]
#+begin_src matlab :exports none
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'ry', 'rz', 'hexa'}
f = logspace(1, 3, 1000);
figure;
for n_stg = 1:2
for n_dir = 1:3
subplot(3, 2, (n_dir-1)*2 + n_stg);
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg+2) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
if n_dir == 3
xlabel('Frequency [Hz]');
end
hold off;
xlim([10, 1000]);
ylim([1e-12, 1e-6]);
end
end
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_comp_mid_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:identification_comp_mid_stages
#+CAPTION: caption ([[./figs/identification_comp_mid_stages.png][png]], [[./figs/identification_comp_mid_stages.pdf][pdf]])
[[file:figs/identification_comp_mid_stages.png]]
#+begin_src matlab :exports none
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'hexa'}
f = logspace(1, 3, 1000);
figure;
for n_stg = 1
for n_dir = 1:3
subplot(3, 1, (n_dir-1) + n_stg);
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg+4) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
if n_dir == 3
xlabel('Frequency [Hz]');
end
hold off;
xlim([10, 1000]);
ylim([1e-12, 1e-6]);
end
end
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_comp_top_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:identification_comp_top_stages
#+CAPTION: caption ([[./figs/identification_comp_top_stages.png][png]], [[./figs/identification_comp_top_stages.pdf][pdf]])
[[file:figs/identification_comp_top_stages.png]]
** Obtained Compliance of the Micro-Station
*** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
#+begin_src matlab
open('nass_model.slx')
#+end_src
*** Initialization
We initialize all the stages with the default parameters.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('type', 'compliance');
#+end_src
We put nothing on top of the micro-hexapod.
#+begin_src matlab
initializeAxisc('type', 'none');
initializeMirror('type', 'none');
initializeNanoHexapod('type', 'none');
initializeSample('type', 'none');
#+end_src
#+begin_src matlab
initializeReferences();
initializeDisturbances();
initializeController();
initializeSimscapeConfiguration();
initializeLoggingConfiguration();
#+end_src
And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point.
The obtained compliance is shown in Figure [[fig:compliance_micro_station]].
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Fm'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform
%% Run the linearization
Gm = linearize(mdl, io, 0);
Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
#+end_src
#+begin_src matlab
save('../meas/micro-station-compliance/mat/model.mat', 'Gm');
#+end_src
#+begin_src matlab :exports none
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
freqs = logspace(1, 3, 1000);
figure;
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gm(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance');
legend('location', 'northwest');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/compliance_micro_station.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:compliance_micro_station
#+caption: Obtained compliance of the Micro-Station ([[./figs/compliance_micro_station.png][png]], [[./figs/compliance_micro_station.pdf][pdf]])
[[file:figs/compliance_micro_station.png]]
** Conclusion
#+begin_important
For such a complex system, we believe that the Simscape Model represents the dynamics of the system with enough fidelity.
#+end_important
* Measurement of Positioning Errors
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/ustation_2_kinematics.m
:END:
<<sec:ustation_kinematics>>
** Introduction :ignore:
[[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org]]
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no :noweb yes
<<m-init-path>>
#+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src
* Simulation of Scientific Experiments
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/.m
:END:
<<sec:ustation_kinematics>>
** Introduction :ignore:
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no :noweb yes
<<m-init-path>>
#+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src
* Estimation of disturbances
* Conclusion
<<sec:uniaxial_conclusion>>
* Bibliography :ignore:
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
* Matlab Functions :noexport:
** Simscape Configuration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeSimscapeConfiguration.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = initializeSimscapeConfiguration(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.gravity logical {mustBeNumericOrLogical} = true
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
conf_simscape = struct();
#+end_src
*** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.gravity
conf_simscape.type = 1;
else
conf_simscape.type = 2;
end
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/conf_simscape.mat', 'file')
save('mat/conf_simscape.mat', 'conf_simscape', '-append');
else
save('mat/conf_simscape.mat', 'conf_simscape');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/conf_simscape.mat', 'file')
save('matlab/mat/conf_simscape.mat', 'conf_simscape', '-append');
else
save('matlab/mat/conf_simscape.mat', 'conf_simscape');
end
end
#+end_src
** Logging Configuration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = initializeLoggingConfiguration(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
conf_log = struct();
#+end_src
*** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.log
case 'none'
conf_log.type = 0;
case 'all'
conf_log.type = 1;
case 'forces'
conf_log.type = 2;
end
#+end_src
*** Sampling Time
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
conf_log.Ts = args.Ts;
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/conf_log.mat', 'file')
save('mat/conf_log.mat', 'conf_log', '-append');
else
save('mat/conf_log.mat', 'conf_log');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/conf_log.mat', 'file')
save('matlab/mat/conf_log.mat', 'conf_log', '-append');
else
save('matlab/mat/conf_log.mat', 'conf_log');
end
end
#+end_src
** Ground
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeGround.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [ground] = initializeGround(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =granite= structure.
#+begin_src matlab
ground = struct();
#+end_src
*** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
case 'none'
ground.type = 0;
case 'rigid'
ground.type = 1;
end
#+end_src
*** Ground Solid properties
:PROPERTIES:
:UNNUMBERED: t
:END:
We set the shape and density of the ground solid element.
#+begin_src matlab
ground.shape = [2, 2, 0.5]; % [m]
ground.density = 2800; % [kg/m3]
#+end_src
*** Rotation Point
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
ground.rot_point = args.rot_point;
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'ground', '-append');
else
save('mat/nass_stages.mat', 'ground');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'ground', '-append');
else
save('matlab/mat/nass_stages.mat', 'ground');
end
end
#+end_src
** Granite
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeGranite.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [granite] = initializeGranite(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
args.sample_pos (1,1) double {mustBeNumeric} = 0.8 % Height of the measurment point [m]
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =granite= structure.
#+begin_src matlab
granite = struct();
#+end_src
*** Add Granite Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
case 'none'
granite.type = 0;
case 'rigid'
granite.type = 1;
case 'flexible'
granite.type = 2;
case 'modal-analysis'
granite.type = 3;
case 'init'
granite.type = 4;
end
#+end_src
*** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Properties of the Material and link to the geometry of the granite.
#+begin_src matlab
granite.density = args.density; % [kg/m3]
granite.STEP = 'granite.STEP';
#+end_src
Z-offset for the initial position of the sample with respect to the granite top surface.
#+begin_src matlab
granite.sample_pos = args.sample_pos; % [m]
#+end_src
*** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
granite.K = args.K; % [N/m]
granite.C = args.C; % [N/(m/s)]
#+end_src
*** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fgm');
granite.Deq = -Fgm'./granite.K;
else
granite.Deq = zeros(6,1);
end
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'granite', '-append');
else
save('mat/nass_stages.mat', 'granite');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'granite', '-append');
else
save('matlab/mat/nass_stages.mat', 'granite');
end
end
#+end_src
** Translation Stage
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeTy.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [ty] = initializeTy(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =ty= structure.
#+begin_src matlab
ty = struct();
#+end_src
*** Add Translation Stage Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
case 'none'
ty.type = 0;
case 'rigid'
ty.type = 1;
case 'flexible'
ty.type = 2;
case 'modal-analysis'
ty.type = 3;
case 'init'
ty.type = 4;
end
#+end_src
*** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Define the density of the materials as well as the geometry (STEP files).
#+begin_src matlab
% Ty Granite frame
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
ty.granite_frame.STEP = 'Ty_Granite_Frame.STEP';
% Guide Translation Ty
ty.guide.density = 7800; % [kg/m3] => 76kg
ty.guide.STEP = 'Ty_Guide.STEP';
% Ty - Guide_Translation12
ty.guide12.density = 7800; % [kg/m3]
ty.guide12.STEP = 'Ty_Guide_12.STEP';
% Ty - Guide_Translation11
ty.guide11.density = 7800; % [kg/m3]
ty.guide11.STEP = 'Ty_Guide_11.STEP';
% Ty - Guide_Translation22
ty.guide22.density = 7800; % [kg/m3]
ty.guide22.STEP = 'Ty_Guide_22.STEP';
% Ty - Guide_Translation21
ty.guide21.density = 7800; % [kg/m3]
ty.guide21.STEP = 'Ty_Guide_21.STEP';
% Ty - Plateau translation
ty.frame.density = 7800; % [kg/m3]
ty.frame.STEP = 'Ty_Stage.STEP';
% Ty Stator Part
ty.stator.density = 5400; % [kg/m3]
ty.stator.STEP = 'Ty_Motor_Stator.STEP';
% Ty Rotor Part
ty.rotor.density = 5400; % [kg/m3]
ty.rotor.STEP = 'Ty_Motor_Rotor.STEP';
#+end_src
*** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
#+end_src
*** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Ftym');
ty.Deq = -Ftym'./ty.K;
else
ty.Deq = zeros(6,1);
end
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'ty', '-append');
else
save('mat/nass_stages.mat', 'ty');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'ty', '-append');
else
save('matlab/mat/nass_stages.mat', 'ty');
end
end
#+end_src
** Tilt Stage
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeRy.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [ry] = initializeRy(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.Ry_init (1,1) double {mustBeNumeric} = 0
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =ry= structure.
#+begin_src matlab
ry = struct();
#+end_src
*** Add Tilt Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
case 'none'
ry.type = 0;
case 'rigid'
ry.type = 1;
case 'flexible'
ry.type = 2;
case 'modal-analysis'
ry.type = 3;
case 'init'
ry.type = 4;
end
#+end_src
*** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Properties of the Material and link to the geometry of the Tilt stage.
#+begin_src matlab
% Ry - Guide for the tilt stage
ry.guide.density = 7800; % [kg/m3]
ry.guide.STEP = 'Tilt_Guide.STEP';
% Ry - Rotor of the motor
ry.rotor.density = 2400; % [kg/m3]
ry.rotor.STEP = 'Tilt_Motor_Axis.STEP';
% Ry - Motor
ry.motor.density = 3200; % [kg/m3]
ry.motor.STEP = 'Tilt_Motor.STEP';
% Ry - Plateau Tilt
ry.stage.density = 7800; % [kg/m3]
ry.stage.STEP = 'Tilt_Stage.STEP';
#+end_src
Z-Offset so that the center of rotation matches the sample center;
#+begin_src matlab
ry.z_offset = 0.58178; % [m]
#+end_src
#+begin_src matlab
ry.Ry_init = args.Ry_init; % [rad]
#+end_src
*** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
#+end_src
*** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fym');
ry.Deq = -Fym'./ry.K;
else
ry.Deq = zeros(6,1);
end
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'ry', '-append');
else
save('mat/nass_stages.mat', 'ry');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'ry', '-append');
else
save('matlab/mat/nass_stages.mat', 'ry');
end
end
#+end_src
** Spindle
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeRz.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [rz] = initializeRz(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =rz= structure.
#+begin_src matlab
rz = struct();
#+end_src
*** Add Spindle Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
case 'none'
rz.type = 0;
case 'rigid'
rz.type = 1;
case 'flexible'
rz.type = 2;
case 'modal-analysis'
rz.type = 3;
case 'init'
rz.type = 4;
end
#+end_src
*** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Properties of the Material and link to the geometry of the spindle.
#+begin_src matlab
% Spindle - Slip Ring
rz.slipring.density = 7800; % [kg/m3]
rz.slipring.STEP = 'Spindle_Slip_Ring.STEP';
% Spindle - Rotor
rz.rotor.density = 7800; % [kg/m3]
rz.rotor.STEP = 'Spindle_Rotor.STEP';
% Spindle - Stator
rz.stator.density = 7800; % [kg/m3]
rz.stator.STEP = 'Spindle_Stator.STEP';
#+end_src
*** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
#+end_src
*** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fzm');
rz.Deq = -Fzm'./rz.K;
else
rz.Deq = zeros(6,1);
end
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'rz', '-append');
else
save('mat/nass_stages.mat', 'rz');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'rz', '-append');
else
save('matlab/mat/nass_stages.mat', 'rz');
end
end
#+end_src
** Micro Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeMicroHexapod.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [micro_hexapod] = initializeMicroHexapod(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
% generateGeneralConfiguration
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
% initializeStrutDynamics
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
% initializeCylindricalPlatforms
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
% initializeCylindricalStruts
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
% Force that stiffness of each joint should apply at t=0
args.Foffset logical {mustBeNumericOrLogical} = false
end
#+end_src
*** Function content
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, ...
'H', args.H, ...
'MO_B', args.MO_B);
stewart = generateGeneralConfiguration(stewart, ...
'FH', args.FH, ...
'FR', args.FR, ...
'FTh', args.FTh, ...
'MH', args.MH, ...
'MR', args.MR, ...
'MTh', args.MTh);
stewart = computeJointsPose(stewart);
#+end_src
#+begin_src matlab
stewart = initializeStrutDynamics(stewart, ...
'K', args.Ki, ...
'C', args.Ci);
stewart = initializeJointDynamics(stewart, ...
'type_F', 'universal_p', ...
'type_M', 'spherical_p');
#+end_src
#+begin_src matlab
stewart = initializeCylindricalPlatforms(stewart, ...
'Fpm', args.Fpm, ...
'Fph', args.Fph, ...
'Fpr', args.Fpr, ...
'Mpm', args.Mpm, ...
'Mph', args.Mph, ...
'Mpr', args.Mpr);
stewart = initializeCylindricalStruts(stewart, ...
'Fsm', args.Fsm, ...
'Fsh', args.Fsh, ...
'Fsr', args.Fsr, ...
'Msm', args.Msm, ...
'Msh', args.Msh, ...
'Msr', args.Msr);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart, ...
'AP', args.AP, ...
'ARB', args.ARB);
#+end_src
#+begin_src matlab
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src
Equilibrium position of the each joint.
#+begin_src matlab
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fhm');
stewart.actuators.dLeq = -Fhm'./args.Ki;
else
stewart.actuators.dLeq = zeros(6,1);
end
#+end_src
*** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
case 'none'
stewart.type = 0;
case 'rigid'
stewart.type = 1;
case 'flexible'
stewart.type = 2;
case 'modal-analysis'
stewart.type = 3;
case 'init'
stewart.type = 4;
case 'compliance'
stewart.type = 5;
end
#+end_src
*** Save the Structure
#+begin_src matlab
micro_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'micro_hexapod', '-append');
else
save('mat/nass_stages.mat', 'micro_hexapod');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_stages.mat', 'file')
save('matlab/mat/nass_stages.mat', 'micro_hexapod', '-append');
else
save('matlab/mat/nass_stages.mat', 'micro_hexapod');
end
end
#+end_src
** Generate Reference Signals
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeReferences.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [ref] = initializeReferences(args)
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
% Sampling Frequency [s]
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
% Maximum simulation time [s]
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
% Either "constant" / "triangular" / "sinusoidal"
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude of the displacement [m]
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "triangular" / "sinusoidal"
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude [rad]
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "rotating"
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
% Initial angle [rad]
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the rotating [s]
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% For now, only constant is implemented
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
% For now, only constant is implemented
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
% Initial position of the two masses
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
% For now, only constant is implemented
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
end
#+end_src
*** Initialize Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Set Sampling Time
Ts = args.Ts;
Tmax = args.Tmax;
%% Low Pass Filter to filter out the references
s = zpk('s');
w0 = 2*pi*10;
xi = 1;
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
#+end_src
*** Translation Stage
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Translation stage - Dy
t = 0:Ts:Tmax; % Time Vector [s]
Dy = zeros(length(t), 1);
Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1);
switch args.Dy_type
case 'constant'
Dy(:) = args.Dy_amplitude;
Dyd(:) = 0;
Dydd(:) = 0;
case 'triangular'
% This is done to unsure that we start with no displacement
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
i0 = find(t>=args.Dy_period/4,1);
Dy(1:end-i0+1) = Dy_raw(i0:end);
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
% The signal is filtered out
Dy = lsim(H_lpf, Dy, t);
Dyd = lsim(H_lpf*s, Dy, t);
Dydd = lsim(H_lpf*s^2, Dy, t);
case 'sinusoidal'
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
otherwise
warning('Dy_type is not set correctly');
end
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
#+end_src
*** Tilt Stage
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Tilt Stage - Ry
t = 0:Ts:Tmax; % Time Vector [s]
Ry = zeros(length(t), 1);
Ryd = zeros(length(t), 1);
Rydd = zeros(length(t), 1);
switch args.Ry_type
case 'constant'
Ry(:) = args.Ry_amplitude;
Ryd(:) = 0;
Rydd(:) = 0;
case 'triangular'
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
i0 = find(t>=args.Ry_period/4,1);
Ry(1:end-i0+1) = Ry_raw(i0:end);
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
% The signal is filtered out
Ry = lsim(H_lpf, Ry, t);
Ryd = lsim(H_lpf*s, Ry, t);
Rydd = lsim(H_lpf*s^2, Ry, t);
case 'sinusoidal'
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
otherwise
warning('Ry_type is not set correctly');
end
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
#+end_src
*** Spindle
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Spindle - Rz
t = 0:Ts:Tmax; % Time Vector [s]
Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1);
switch args.Rz_type
case 'constant'
Rz(:) = args.Rz_amplitude;
Rzd(:) = 0;
Rzdd(:) = 0;
case 'rotating-not-filtered'
Rz(:) = 2*pi/args.Rz_period*t;
% The signal is filtered out
Rz(:) = 2*pi/args.Rz_period*t;
Rzd(:) = 2*pi/args.Rz_period;
Rzdd(:) = 0;
% We add the angle offset
Rz = Rz + args.Rz_amplitude;
case 'rotating'
Rz(:) = 2*pi/args.Rz_period*t;
% The signal is filtered out
Rz = lsim(H_lpf, Rz, t);
Rzd = lsim(H_lpf*s, Rz, t);
Rzdd = lsim(H_lpf*s^2, Rz, t);
% We add the angle offset
Rz = Rz + args.Rz_amplitude;
otherwise
warning('Rz_type is not set correctly');
end
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
#+end_src
*** Micro Hexapod
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Micro-Hexapod
t = [0, Ts];
Dh = zeros(length(t), 6);
Dhl = zeros(length(t), 6);
switch args.Dh_type
case 'constant'
Dh = [args.Dh_pos, args.Dh_pos];
load('nass_stages.mat', 'micro_hexapod');
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
tx = args.Dh_pos(4);
ty = args.Dh_pos(5);
tz = args.Dh_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
Dhl = [Dhl, Dhl];
otherwise
warning('Dh_type is not set correctly');
end
Dh = struct('time', t, 'signals', struct('values', Dh));
Dhl = struct('time', t, 'signals', struct('values', Dhl));
#+end_src
*** Axis Compensation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Axis Compensation - Rm
t = [0, Ts];
Rm = [args.Rm_pos, args.Rm_pos];
Rm = struct('time', t, 'signals', struct('values', Rm));
#+end_src
*** Nano Hexapod
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Nano-Hexapod
t = [0, Ts];
Dn = zeros(length(t), 6);
switch args.Dn_type
case 'constant'
Dn = [args.Dn_pos, args.Dn_pos];
load('nass_stages.mat', 'nano_hexapod');
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
tx = args.Dn_pos(4);
ty = args.Dn_pos(5);
tz = args.Dn_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
Dnl = [Dnl, Dnl];
otherwise
warning('Dn_type is not set correctly');
end
Dn = struct('time', t, 'signals', struct('values', Dn));
Dnl = struct('time', t, 'signals', struct('values', Dnl));
#+end_src
*** Save the Structure
#+begin_src matlab
micro_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nass_references.mat', 'file')
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
else
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_references.mat', 'file')
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
else
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
end
end
#+end_src
** Initialize Disturbances
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeDisturbances.m
:header-args:matlab+: :comments none :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
*** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = initializeDisturbances(args)
% initializeDisturbances - Initialize the disturbances
%
% Syntax: [] = initializeDisturbances(args)
%
% Inputs:
% - args -
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
% Global parameter to enable or disable the disturbances
args.enable logical {mustBeNumericOrLogical} = true
% Ground Motion - X direction
args.Dwx logical {mustBeNumericOrLogical} = true
% Ground Motion - Y direction
args.Dwy logical {mustBeNumericOrLogical} = true
% Ground Motion - Z direction
args.Dwz logical {mustBeNumericOrLogical} = true
% Translation Stage - X direction
args.Fty_x logical {mustBeNumericOrLogical} = true
% Translation Stage - Z direction
args.Fty_z logical {mustBeNumericOrLogical} = true
% Spindle - Z direction
args.Frz_z logical {mustBeNumericOrLogical} = true
end
#+end_src
*** Load Data
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
load('./mat/dist_psd.mat', 'dist_f');
#+end_src
We remove the first frequency point that usually is very large.
#+begin_src matlab :exports none
dist_f.f = dist_f.f(2:end);
dist_f.psd_gm = dist_f.psd_gm(2:end);
dist_f.psd_ty = dist_f.psd_ty(2:end);
dist_f.psd_rz = dist_f.psd_rz(2:end);
#+end_src
*** Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
We define some parameters that will be used in the algorithm.
#+begin_src matlab
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
T0 = N/Fs; % Signal Duration [s]
df = 1/T0; % Frequency resolution of the DFT [Hz]
% Also equal to (dist_f.f(2)-dist_f.f(1))
t = linspace(0, T0, N+1)'; % Time Vector [s]
Ts = 1/Fs; % Sampling Time [s]
#+end_src
*** Ground Motion
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
phi = dist_f.psd_gm;
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
#+end_src
#+begin_src matlab
if args.Dwx && args.enable
rng(111);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
else
Dwx = zeros(length(t), 1);
end
#+end_src
#+begin_src matlab
if args.Dwy && args.enable
rng(112);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
else
Dwy = zeros(length(t), 1);
end
#+end_src
#+begin_src matlab
if args.Dwy && args.enable
rng(113);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
else
Dwz = zeros(length(t), 1);
end
#+end_src
*** Translation Stage - X direction
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Fty_x && args.enable
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
rng(121);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
Fty_x = u;
else
Fty_x = zeros(length(t), 1);
end
#+end_src
*** Translation Stage - Z direction
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Fty_z && args.enable
phi = dist_f.psd_ty;
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
rng(122);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
Fty_z = u;
else
Fty_z = zeros(length(t), 1);
end
#+end_src
*** Spindle - Z direction
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Frz_z && args.enable
phi = dist_f.psd_rz;
C = zeros(N/2,1);
for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
rng(131);
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
Frz_z = u;
else
Frz_z = zeros(length(t), 1);
end
#+end_src
*** Direct Forces
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
u = zeros(length(t), 6);
Fd = u;
#+end_src
*** Set initial value to zero
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
Dwx = Dwx - Dwx(1);
Dwy = Dwy - Dwy(1);
Dwz = Dwz - Dwz(1);
Fty_x = Fty_x - Fty_x(1);
Fty_z = Fty_z - Fty_z(1);
Frz_z = Frz_z - Frz_z(1);
#+end_src
*** Save the Structure
#+begin_src matlab
micro_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nass_disturbances.mat', 'file')
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args', '-append');
else
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_disturbances.mat', 'file')
save('matlab/mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args', '-append');
else
save('matlab/mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
end
end
#+end_src
** Z-Axis Geophone
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeZAxisGeophone.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
#+begin_src matlab
function [geophone] = initializeZAxisGeophone(args)
arguments
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz]
end
%%
geophone.m = args.mass;
%% The Stiffness is set to have the damping resonance frequency
geophone.k = geophone.m * (2*pi*args.freq)^2;
%% We set the damping value to have critical damping
geophone.c = 2*sqrt(geophone.m * geophone.k);
#+end_src
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/geophone_z_axis.mat', 'file')
save('mat/geophone_z_axis.mat', 'geophone', '-append');
else
save('mat/geophone_z_axis.mat', 'geophone');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/geophone_z_axis.mat', 'file')
save('matlab/mat/geophone_z_axis.mat', 'geophone', '-append');
else
save('matlab/mat/geophone_z_axis.mat', 'geophone');
end
end
#+end_src
** Z-Axis Accelerometer
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeZAxisAccelerometer.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
#+begin_src matlab
function [accelerometer] = initializeZAxisAccelerometer(args)
arguments
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz]
end
%%
accelerometer.m = args.mass;
%% The Stiffness is set to have the damping resonance frequency
accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;
%% We set the damping value to have critical damping
accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);
%% Gain correction of the accelerometer to have a unity gain until the resonance
accelerometer.gain = -accelerometer.k/accelerometer.m;
#+end_src
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/accelerometer_z_axis.mat', 'file')
save('mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
else
save('mat/accelerometer_z_axis.mat', 'accelerometer');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/accelerometer_z_axis.mat', 'file')
save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
else
save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer');
end
end
#+end_src
* Helping Functions :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
addpath('./matlab/'); % Path for scripts
%% Path for functions, data and scripts
addpath('./matlab/mat/'); % Path for Computed FRF
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/STEPS/'); % Path for STEPS
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
#+END_SRC
#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files
#+END_SRC
** Initialize Simscape Model
#+NAME: m-init-simscape
#+begin_src matlab
% Simulink Model name
mdl = 'ustation_simscape';
#+end_src
** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);
#+END_SRC
Reference in New Issue View Git Blame Copy Permalink