2019-07-12 11:53:59 +02:00
<?xml version="1.0" encoding="utf-8"?>
< !DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
< html xmlns = "http://www.w3.org/1999/xhtml" lang = "en" xml:lang = "en" >
< head >
2019-10-08 11:03:19 +02:00
<!-- 2019 - 07 - 15 lun. 11:07 -->
2019-07-12 11:53:59 +02:00
< meta http-equiv = "Content-Type" content = "text/html;charset=utf-8" / >
< meta name = "viewport" content = "width=device-width, initial-scale=1" / >
< title > Simscape Model of the Nano-Active-Stabilization-System< / title >
< meta name = "generator" content = "Org mode" / >
< meta name = "author" content = "Dehaeze Thomas" / >
< style type = "text/css" >
<!-- /* --> <![CDATA[/*> <!-- */
.title { text-align: center;
margin-bottom: .2em; }
.subtitle { text-align: center;
font-size: medium;
font-weight: bold;
margin-top:0; }
.todo { font-family: monospace; color: red; }
.done { font-family: monospace; color: green; }
.priority { font-family: monospace; color: orange; }
.tag { background-color: #eee; font-family: monospace;
padding: 2px; font-size: 80%; font-weight: normal; }
.timestamp { color: #bebebe; }
.timestamp-kwd { color: #5f9ea0; }
.org-right { margin-left: auto; margin-right: 0px; text-align: right; }
.org-left { margin-left: 0px; margin-right: auto; text-align: left; }
.org-center { margin-left: auto; margin-right: auto; text-align: center; }
.underline { text-decoration: underline; }
#postamble p, #preamble p { font-size: 90%; margin: .2em; }
p.verse { margin-left: 3%; }
pre {
border: 1px solid #ccc;
box-shadow: 3px 3px 3px #eee;
padding: 8pt;
font-family: monospace;
overflow: auto;
margin: 1.2em;
}
pre.src {
position: relative;
overflow: visible;
padding-top: 1.2em;
}
pre.src:before {
display: none;
position: absolute;
background-color: white;
top: -10px;
right: 10px;
padding: 3px;
border: 1px solid black;
}
pre.src:hover:before { display: inline;}
/* Languages per Org manual */
pre.src-asymptote:before { content: 'Asymptote'; }
pre.src-awk:before { content: 'Awk'; }
pre.src-C:before { content: 'C'; }
/* pre.src-C++ doesn't work in CSS */
pre.src-clojure:before { content: 'Clojure'; }
pre.src-css:before { content: 'CSS'; }
pre.src-D:before { content: 'D'; }
pre.src-ditaa:before { content: 'ditaa'; }
pre.src-dot:before { content: 'Graphviz'; }
pre.src-calc:before { content: 'Emacs Calc'; }
pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
pre.src-fortran:before { content: 'Fortran'; }
pre.src-gnuplot:before { content: 'gnuplot'; }
pre.src-haskell:before { content: 'Haskell'; }
pre.src-hledger:before { content: 'hledger'; }
pre.src-java:before { content: 'Java'; }
pre.src-js:before { content: 'Javascript'; }
pre.src-latex:before { content: 'LaTeX'; }
pre.src-ledger:before { content: 'Ledger'; }
pre.src-lisp:before { content: 'Lisp'; }
pre.src-lilypond:before { content: 'Lilypond'; }
pre.src-lua:before { content: 'Lua'; }
pre.src-matlab:before { content: 'MATLAB'; }
pre.src-mscgen:before { content: 'Mscgen'; }
pre.src-ocaml:before { content: 'Objective Caml'; }
pre.src-octave:before { content: 'Octave'; }
pre.src-org:before { content: 'Org mode'; }
pre.src-oz:before { content: 'OZ'; }
pre.src-plantuml:before { content: 'Plantuml'; }
pre.src-processing:before { content: 'Processing.js'; }
pre.src-python:before { content: 'Python'; }
pre.src-R:before { content: 'R'; }
pre.src-ruby:before { content: 'Ruby'; }
pre.src-sass:before { content: 'Sass'; }
pre.src-scheme:before { content: 'Scheme'; }
pre.src-screen:before { content: 'Gnu Screen'; }
pre.src-sed:before { content: 'Sed'; }
pre.src-sh:before { content: 'shell'; }
pre.src-sql:before { content: 'SQL'; }
pre.src-sqlite:before { content: 'SQLite'; }
/* additional languages in org.el's org-babel-load-languages alist */
pre.src-forth:before { content: 'Forth'; }
pre.src-io:before { content: 'IO'; }
pre.src-J:before { content: 'J'; }
pre.src-makefile:before { content: 'Makefile'; }
pre.src-maxima:before { content: 'Maxima'; }
pre.src-perl:before { content: 'Perl'; }
pre.src-picolisp:before { content: 'Pico Lisp'; }
pre.src-scala:before { content: 'Scala'; }
pre.src-shell:before { content: 'Shell Script'; }
pre.src-ebnf2ps:before { content: 'ebfn2ps'; }
/* additional language identifiers per "defun org-babel-execute"
in ob-*.el */
pre.src-cpp:before { content: 'C++'; }
pre.src-abc:before { content: 'ABC'; }
pre.src-coq:before { content: 'Coq'; }
pre.src-groovy:before { content: 'Groovy'; }
/* additional language identifiers from org-babel-shell-names in
ob-shell.el: ob-shell is the only babel language using a lambda to put
the execution function name together. */
pre.src-bash:before { content: 'bash'; }
pre.src-csh:before { content: 'csh'; }
pre.src-ash:before { content: 'ash'; }
pre.src-dash:before { content: 'dash'; }
pre.src-ksh:before { content: 'ksh'; }
pre.src-mksh:before { content: 'mksh'; }
pre.src-posh:before { content: 'posh'; }
/* Additional Emacs modes also supported by the LaTeX listings package */
pre.src-ada:before { content: 'Ada'; }
pre.src-asm:before { content: 'Assembler'; }
pre.src-caml:before { content: 'Caml'; }
pre.src-delphi:before { content: 'Delphi'; }
pre.src-html:before { content: 'HTML'; }
pre.src-idl:before { content: 'IDL'; }
pre.src-mercury:before { content: 'Mercury'; }
pre.src-metapost:before { content: 'MetaPost'; }
pre.src-modula-2:before { content: 'Modula-2'; }
pre.src-pascal:before { content: 'Pascal'; }
pre.src-ps:before { content: 'PostScript'; }
pre.src-prolog:before { content: 'Prolog'; }
pre.src-simula:before { content: 'Simula'; }
pre.src-tcl:before { content: 'tcl'; }
pre.src-tex:before { content: 'TeX'; }
pre.src-plain-tex:before { content: 'Plain TeX'; }
pre.src-verilog:before { content: 'Verilog'; }
pre.src-vhdl:before { content: 'VHDL'; }
pre.src-xml:before { content: 'XML'; }
pre.src-nxml:before { content: 'XML'; }
/* add a generic configuration mode; LaTeX export needs an additional
(add-to-list 'org-latex-listings-langs '(conf " ")) in .emacs */
pre.src-conf:before { content: 'Configuration File'; }
table { border-collapse:collapse; }
caption.t-above { caption-side: top; }
caption.t-bottom { caption-side: bottom; }
td, th { vertical-align:top; }
th.org-right { text-align: center; }
th.org-left { text-align: center; }
th.org-center { text-align: center; }
td.org-right { text-align: right; }
td.org-left { text-align: left; }
td.org-center { text-align: center; }
dt { font-weight: bold; }
.footpara { display: inline; }
.footdef { margin-bottom: 1em; }
.figure { padding: 1em; }
.figure p { text-align: center; }
.equation-container {
display: table;
text-align: center;
width: 100%;
}
.equation {
vertical-align: middle;
}
.equation-label {
display: table-cell;
text-align: right;
vertical-align: middle;
}
.inlinetask {
padding: 10px;
border: 2px solid gray;
margin: 10px;
background: #ffffcc;
}
#org-div-home-and-up
{ text-align: right; font-size: 70%; white-space: nowrap; }
textarea { overflow-x: auto; }
.linenr { font-size: smaller }
.code-highlighted { background-color: #ffff00; }
.org-info-js_info-navigation { border-style: none; }
#org-info-js_console-label
{ font-size: 10px; font-weight: bold; white-space: nowrap; }
.org-info-js_search-highlight
{ background-color: #ffff00; color: #000000; font-weight: bold; }
.org-svg { width: 90%; }
/*]]>*/-->
< / style >
< link rel = "stylesheet" type = "text/css" href = "./css/htmlize.css" / >
< link rel = "stylesheet" type = "text/css" href = "./css/readtheorg.css" / >
< link rel = "stylesheet" type = "text/css" href = "./css/zenburn.css" / >
< script type = "text/javascript" src = "./js/jquery.min.js" > < / script >
< script type = "text/javascript" src = "./js/bootstrap.min.js" > < / script >
< script type = "text/javascript" src = "./js/jquery.stickytableheaders.min.js" > < / script >
< script type = "text/javascript" src = "./js/readtheorg.js" > < / script >
< script type = "text/javascript" >
/*
@licstart The following is the entire license notice for the
JavaScript code in this tag.
Copyright (C) 2012-2019 Free Software Foundation, Inc.
The JavaScript code in this tag is free software: you can
redistribute it and/or modify it under the terms of the GNU
General Public License (GNU GPL) as published by the Free Software
Foundation, either version 3 of the License, or (at your option)
any later version. The code is distributed WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
As additional permission under GNU GPL version 3 section 7, you
may distribute non-source (e.g., minimized or compacted) forms of
that code without the copy of the GNU GPL normally required by
section 4, provided you include this license notice and a URL
through which recipients can access the Corresponding Source.
@licend The above is the entire license notice
for the JavaScript code in this tag.
*/
<!-- /* --> <![CDATA[/*> <!-- */
function CodeHighlightOn(elem, id)
{
var target = document.getElementById(id);
if(null != target) {
elem.cacheClassElem = elem.className;
elem.cacheClassTarget = target.className;
target.className = "code-highlighted";
elem.className = "code-highlighted";
}
}
function CodeHighlightOff(elem, id)
{
var target = document.getElementById(id);
if(elem.cacheClassElem)
elem.className = elem.cacheClassElem;
if(elem.cacheClassTarget)
target.className = elem.cacheClassTarget;
}
/*]]>*///-->
< / script >
< / head >
< body >
2019-10-08 11:03:19 +02:00
< div id = "org-div-home-and-up" >
< a accesskey = "h" href = "./index.html" > UP < / a >
|
< a accesskey = "H" href = "../index.html" > HOME < / a >
< / div > < div id = "content" >
2019-07-12 11:53:59 +02:00
< h1 class = "title" > Simscape Model of the Nano-Active-Stabilization-System< / h1 >
2019-10-08 11:03:19 +02:00
< div id = "table-of-contents" >
< h2 > Table of Contents< / h2 >
< div id = "text-table-of-contents" >
< ul >
< li > < a href = "#org52cbbef" > 1. Simulink project< / a > < / li >
< li > < a href = "#orgcef8ec0" > 2. Notes< / a >
< ul >
< li > < a href = "#orgf4fe710" > 2.1. Simscape files for identification< / a > < / li >
< li > < a href = "#org2c35682" > 2.2. Inputs< / a >
< ul >
< li > < a href = "#org2ebe76f" > 2.2.1. Perturbations< / a > < / li >
< li > < a href = "#org040ce5f" > 2.2.2. Measurement Noise< / a > < / li >
< li > < a href = "#org59020ea" > 2.2.3. Control Inputs< / a > < / li >
< / ul >
< / li >
< li > < a href = "#org82ad322" > 2.3. Outputs< / a > < / li >
< / ul >
< / li >
< li > < a href = "#orga3ca676" > 3. Simulink files< / a > < / li >
< li > < a href = "#org39790fd" > 4. Simulink Library< / a >
< ul >
< li > < a href = "#orgfa5da1a" > 4.1. inputs< / a > < / li >
< li > < a href = "#org257f870" > 4.2. nass< sub > library< / sub > < / a > < / li >
< li > < a href = "#org2b86bdc" > 4.3. pos< sub > error< / sub > < sub > wrt< / sub > < sub > nass< / sub > < sub > base< / sub > < / a > < / li >
< li > < a href = "#org208901a" > 4.4. QuaternionToAngles< / a > < / li >
< li > < a href = "#org50380fa" > 4.5. RotationMatrixToAngle< / a > < / li >
< / ul >
< / li >
< li > < a href = "#orgd59e594" > 5. Scripts< / a >
< ul >
< li > < a href = "#orgc39ddce" > 5.1. Simulation Initialization< / a > < / li >
< / ul >
< / li >
< li > < a href = "#org196dc65" > 6. Functions< / a >
< ul >
< li > < a href = "#org2ad7bbc" > 6.1. computePsdDispl< / a > < / li >
< li > < a href = "#org0983e5e" > 6.2. computeSetpoint< / a > < / li >
< li > < a href = "#org8f3b4fc" > 6.3. converErrorBasis< / a > < / li >
< li > < a href = "#orgff6efdf" > 6.4. generateDiagPidControl< / a > < / li >
< li > < a href = "#org00bd715" > 6.5. identifyPlant< / a > < / li >
< li > < a href = "#org618b4fa" > 6.6. runSimulation< / a > < / li >
< / ul >
< / li >
< li > < a href = "#org0cf6746" > 7. Initialize Elements< / a >
< ul >
< li > < a href = "#org25ddf30" > 7.1. Simulation Configuration< / a > < / li >
< li > < a href = "#org4ab253c" > 7.2. Experiment< / a > < / li >
< li > < a href = "#org410da45" > 7.3. Inputs< / a > < / li >
< li > < a href = "#org05c393c" > 7.4. Ground< / a > < / li >
< li > < a href = "#orgd3a8362" > 7.5. Granite< / a > < / li >
< li > < a href = "#org534ce7a" > 7.6. Translation Stage< / a > < / li >
< li > < a href = "#orgf227dfc" > 7.7. Tilt Stage< / a > < / li >
< li > < a href = "#orge76075d" > 7.8. Spindle< / a > < / li >
< li > < a href = "#org27dbc96" > 7.9. Micro Hexapod< / a > < / li >
< li > < a href = "#org4e5c6e0" > 7.10. Center of gravity compensation< / a > < / li >
< li > < a href = "#orgc5a4039" > 7.11. Mirror< / a > < / li >
< li > < a href = "#orgaf1e477" > 7.12. Nano Hexapod< / a > < / li >
< li > < a href = "#org33550ef" > 7.13. Sample< / a > < / li >
< / ul >
< / li >
< / ul >
< / div >
< / div >
< ul class = "org-ul" >
< li > < a href = "identification/index.html" > Identification of the Micro-Station< / a > < / li >
< li > < a href = "kinematics/index.html" > Kinematics of the station< / a > < / li >
< li > Control< / li >
< / ul >
< div id = "outline-container-org52cbbef" class = "outline-2" >
< h2 id = "org52cbbef" > < span class = "section-number-2" > 1< / span > Simulink project< / h2 >
< div class = "outline-text-2" id = "text-1" >
< p >
The project can be opened using the < code > simulinkproject< / code > function:
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > simulinkproject< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< p >
When the project opens, a startup script is ran.
The startup script is defined below and is exported to the < code > project_startup.m< / code > script.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
freqs = logspace< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
save_fig = false;
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/config.mat', 'freqs', 'save_fig'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
project = simulinkproject;
projectRoot = project.RootFolder;
myCacheFolder = fullfile< span class = "org-rainbow-delimiters-depth-1" > (< / span > projectRoot, < span class = "org-string" > '.SimulinkCache'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
myCodeFolder = fullfile< span class = "org-rainbow-delimiters-depth-1" > (< / span > projectRoot, < span class = "org-string" > '.SimulinkCode'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Simulink.fileGenControl< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'set'< / span > ,< span class = "org-underline" > ...< / span >
< span class = "org-string" > 'CacheFolder'< / span > , myCacheFolder,< span class = "org-underline" > ...< / span >
< span class = "org-string" > 'CodeGenFolder'< / span > , myCodeFolder,< span class = "org-underline" > ...< / span >
< span class = "org-string" > 'createDir'< / span > , true< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< p >
When the project closes, it runs the < code > project_shutdown.m< / code > script defined below.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Simulink.fileGenControl< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'reset'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< p >
The project also permits to automatically add defined folder to the path when the project is opened.
< / p >
< / div >
< / div >
< div id = "outline-container-orgcef8ec0" class = "outline-2" >
< h2 id = "orgcef8ec0" > < span class = "section-number-2" > 2< / span > Notes< / h2 >
< div class = "outline-text-2" id = "text-2" >
< p >
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.
< / p >
< / div >
< div id = "outline-container-orgf4fe710" class = "outline-3" >
< h3 id = "orgf4fe710" > < span class = "section-number-3" > 2.1< / span > Simscape files for identification< / h3 >
< div class = "outline-text-3" id = "text-2-1" >
< table border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Simscape Name< / th >
< th scope = "col" class = "org-left" > Ty< / th >
< th scope = "col" class = "org-left" > Ry< / th >
< th scope = "col" class = "org-left" > Rz< / th >
< th scope = "col" class = "org-left" > Hexa< / th >
< th scope = "col" class = "org-left" > NASS< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > id micro station< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > id nano station stages< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< / tr >
< tr >
< td class = "org-left" > id nano station config< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > F< / td >
< / tr >
< tr >
< td class = "org-left" > control nano station< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > F< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
< div id = "outline-container-org2c35682" class = "outline-3" >
< h3 id = "org2c35682" > < span class = "section-number-3" > 2.2< / span > Inputs< / h3 >
< div class = "outline-text-3" id = "text-2-2" >
< / div >
< div id = "outline-container-org2ebe76f" class = "outline-4" >
< h4 id = "org2ebe76f" > < span class = "section-number-4" > 2.2.1< / span > Perturbations< / h4 >
< div class = "outline-text-4" id = "text-2-2-1" >
< table border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-right" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > < code > Dw< / code > < / td >
< td class = "org-left" > Ground motion< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fg< / code > < / td >
< td class = "org-left" > External force applied on granite< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fs< / code > < / td >
< td class = "org-left" > External force applied on the Sample< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
< div id = "outline-container-org040ce5f" class = "outline-4" >
< h4 id = "org040ce5f" > < span class = "section-number-4" > 2.2.2< / span > Measurement Noise< / h4 >
< div class = "outline-text-4" id = "text-2-2-2" >
< table border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-left" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
< div id = "outline-container-org59020ea" class = "outline-4" >
< h4 id = "org59020ea" > < span class = "section-number-4" > 2.2.3< / span > Control Inputs< / h4 >
< div class = "outline-text-4" id = "text-2-2-3" >
< table border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-right" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > < code > Fy< / code > < / td >
< td class = "org-left" > Actuation force for Ty< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dy< / code > < / td >
< td class = "org-left" > Imposed displacement for Ty< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > My< / code > < / td >
< td class = "org-left" > Actuation torque for Ry< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Ry< / code > < / td >
< td class = "org-left" > Imposed rotation for Ry< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Mz< / code > < / td >
< td class = "org-left" > Actuation torque for Rz< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Rz< / code > < / td >
< td class = "org-left" > Imposed rotation for Rz< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Fh< / code > < / td >
< td class = "org-left" > Actuation force/torque for hexapod (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N, N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fhl< / code > < / td >
< td class = "org-left" > Actuation force/torque for hexapod (legs)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dh< / code > < / td >
< td class = "org-left" > Imposed position for hexapod (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Rm< / code > < / td >
< td class = "org-left" > Position of the two masses< / td >
< td class = "org-right" > 2< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Fn< / code > < / td >
< td class = "org-left" > Actuation force for the NASS (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N, N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fnl< / code > < / td >
< td class = "org-left" > Actuation force for the NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dn< / code > < / td >
< td class = "org-left" > Imposed position for the NASS (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
< / div >
< div id = "outline-container-org82ad322" class = "outline-3" >
< h3 id = "org82ad322" > < span class = "section-number-3" > 2.3< / span > Outputs< / h3 >
< div class = "outline-text-3" id = "text-2-3" >
< table border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-right" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > < code > Dgm< / code > < / td >
< td class = "org-left" > Absolute displacement of the granite< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Vgm< / code > < / td >
< td class = "org-left" > Absolute Velocity of the granite< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [m/s]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Dym< / code > < / td >
< td class = "org-left" > Measured displacement of Ty< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Rym< / code > < / td >
< td class = "org-left" > Measured rotation of Ry< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Rzm< / code > < / td >
< td class = "org-left" > Measured rotation of Rz< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Dhm< / code > < / td >
< td class = "org-left" > Measured position of hexapod (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Fnlm< / code > < / td >
< td class = "org-left" > Measured force of NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dnlm< / code > < / td >
< td class = "org-left" > Measured elongation of NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dnm< / code > < / td >
< td class = "org-left" > Measured position of NASS w.r.t NASS's base< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Vnm< / code > < / td >
< td class = "org-left" > Measured absolute velocity of NASS platform< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m/s, rad/s]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Vnlm< / code > < / td >
< td class = "org-left" > Measured absolute velocity of NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m/s]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Dsm< / code > < / td >
< td class = "org-left" > Position of Sample w.r.t. granite frame< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
< / div >
< div id = "outline-container-orga3ca676" class = "outline-2" >
< h2 id = "orga3ca676" > < span class = "section-number-2" > 3< / span > Simulink files< / h2 >
< div class = "outline-text-2" id = "text-3" >
< p >
Few different Simulink files are used:
< / p >
< ul class = "org-ul" >
< li > kinematics< / li >
< li > identification - micro station< / li >
< li > identification - nano station< / li >
< li > control< / li >
< / ul >
< / div >
< / div >
< div id = "outline-container-org39790fd" class = "outline-2" >
< h2 id = "org39790fd" > < span class = "section-number-2" > 4< / span > Simulink Library< / h2 >
< div class = "outline-text-2" id = "text-4" >
< p >
A simulink library is developed in order to share elements between the different simulink files.
< / p >
< / div >
< div id = "outline-container-orgfa5da1a" class = "outline-3" >
< h3 id = "orgfa5da1a" > < span class = "section-number-3" > 4.1< / span > inputs< / h3 >
< / div >
< div id = "outline-container-org257f870" class = "outline-3" >
< h3 id = "org257f870" > < span class = "section-number-3" > 4.2< / span > nass< sub > library< / sub > < / h3 >
< / div >
< div id = "outline-container-org2b86bdc" class = "outline-3" >
< h3 id = "org2b86bdc" > < span class = "section-number-3" > 4.3< / span > pos< sub > error< / sub > < sub > wrt< / sub > < sub > nass< / sub > < sub > base< / sub > < / h3 >
< / div >
< div id = "outline-container-org208901a" class = "outline-3" >
< h3 id = "org208901a" > < span class = "section-number-3" > 4.4< / span > QuaternionToAngles< / h3 >
< / div >
< div id = "outline-container-org50380fa" class = "outline-3" >
< h3 id = "org50380fa" > < span class = "section-number-3" > 4.5< / span > RotationMatrixToAngle< / h3 >
< / div >
< / div >
< div id = "outline-container-orgd59e594" class = "outline-2" >
< h2 id = "orgd59e594" > < span class = "section-number-2" > 5< / span > Scripts< / h2 >
< div class = "outline-text-2" id = "text-5" >
< / div >
< div id = "outline-container-orgc39ddce" class = "outline-3" >
< h3 id = "orgc39ddce" > < span class = "section-number-3" > 5.1< / span > Simulation Initialization< / h3 >
< div class = "outline-text-3" id = "text-5-1" >
< p >
< a id = "org9d78b46" > < / a >
< / p >
< p >
This Matlab script is accessible < a href = "src/init_simulation.m" > here< / a > .
< / p >
< p >
This script runs just before the simulation is started.
It is used to load the simulation configuration and the controllers used for the simulation.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Load all the data used for the simulation< / span > < / span >
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/sim_conf.mat'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Load Controller< / span > < / span >
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< / div >
< / div >
< / div >
< div id = "outline-container-org196dc65" class = "outline-2" >
< h2 id = "org196dc65" > < span class = "section-number-2" > 6< / span > Functions< / h2 >
< div class = "outline-text-2" id = "text-6" >
< / div >
< div id = "outline-container-org2ad7bbc" class = "outline-3" >
< h3 id = "org2ad7bbc" > < span class = "section-number-3" > 6.1< / span > computePsdDispl< / h3 >
< div class = "outline-text-3" id = "text-6-1" >
< p >
< a id = "org1e3e622" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/computePsdDispl.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > psd_object< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > computePsdDispl< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > sys_data< / span > , < span class = "org-variable-name" > t_init< / span > , < span class = "org-variable-name" > n_av< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
i_init = find< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.time < span class = "org-type" > > < / span > t_init, < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
han_win = hanning< span class = "org-rainbow-delimiters-depth-1" > (< / span > ceil< span class = "org-rainbow-delimiters-depth-2" > (< / span > length< span class = "org-rainbow-delimiters-depth-3" > (< / span > sys_data.Dx< span class = "org-rainbow-delimiters-depth-4" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-4" > )< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > /< / span > n_av< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Fs = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > sys_data.time< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pdx, f< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Dx< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pdy, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Dy< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pdz, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Dz< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > prx, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Rx< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pry, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Ry< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > prz, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Rz< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
psd_object = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'f'< / span > , f, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'dx'< / span > , pdx, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'dy'< / span > , pdy, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'dz'< / span > , pdz, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'rx'< / span > , prx, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'ry'< / span > , pry, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'rz'< / span > , prz< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org0983e5e" class = "outline-3" >
< h3 id = "org0983e5e" > < span class = "section-number-3" > 6.2< / span > computeSetpoint< / h3 >
< div class = "outline-text-3" id = "text-6-2" >
< p >
< a id = "org5f760fe" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/computeSetpoint.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > setpoint< / span > = < span class = "org-function-name" > computeSetpoint< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > ty< / span > , < span class = "org-variable-name" > ry< / span > , < span class = "org-variable-name" > rz< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
setpoint = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ty< / span > < / span >
Ty = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ty ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
% Tyinv = [< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
% < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-type" > -< / span > ty ;
% < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
% < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > ];
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ry< / span > < / span >
Ry = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % TMry = Ty*Ry*Tyinv;< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Rz< / span > < / span >
Rz = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % TMrz = Ty*TMry*Rz*TMry'*Tyinv;< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% All stages< / span > < / span >
< span class = "org-comment" > % < / span > < span class = "org-comment" > < span class = "org-constant" > TM < / span > < / span > < span class = "org-comment" > = TMrz*TMry*Ty;< / span >
TM = Ty< span class = "org-type" > *< / span > Ry< span class = "org-type" > *< / span > Rz;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = RM2angle< span class = "org-rainbow-delimiters-depth-1" > (< / span > TM< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = TM< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > thetax, thetay, thetaz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > RM2angle< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > R< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > abs< span class = "org-rainbow-delimiters-depth-2" > (< / span > R< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > > < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 < span class = "org-type" > !< / span > = < span class = "org-highlight-numbers-number" > 1< / span > and R31 < span class = "org-type" > !< / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-type" > -< / span > asin< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetax = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetaz = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
thetaz = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
thetay = < span class = "org-type" > -< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = < span class = "org-type" > -< / span > thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org8f3b4fc" class = "outline-3" >
< h3 id = "org8f3b4fc" > < span class = "section-number-3" > 6.3< / span > converErrorBasis< / h3 >
< div class = "outline-text-3" id = "text-6-3" >
< p >
< a id = "orgf1df33d" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/converErrorBasis.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > error_nass< / span > = < span class = "org-function-name" > convertErrorBasis< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > pos< / span > , < span class = "org-variable-name" > setpoint< / span > , < span class = "org-variable-name" > ty< / span > , < span class = "org-variable-name" > ry< / span > , < span class = "org-variable-name" > rz< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-comment" > % convertErrorBasis -< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Syntax: convertErrorBasis(p_error, ty, ry, rz)< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Inputs:< / span >
< span class = "org-comment" > % - p_error - Position error of the sample w.r.t. the granite [m, rad]< / span >
< span class = "org-comment" > % - ty - Measured translation of the Ty stage [m]< / span >
< span class = "org-comment" > % - ry - Measured rotation of the Ry stage [rad]< / span >
< span class = "org-comment" > % - rz - Measured rotation of the Rz stage [rad]< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Outputs:< / span >
< span class = "org-comment" > % - P_nass - Position error of the sample w.r.t. the NASS base [m]< / span >
< span class = "org-comment" > % - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Example:< / span >
< span class = "org-comment" > %< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% If line vector => column vector< / span > < / span >
< span class = "org-keyword" > if< / span > size< span class = "org-rainbow-delimiters-depth-1" > (< / span > pos, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 6< / span >
pos = pos';
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > if< / span > size< span class = "org-rainbow-delimiters-depth-1" > (< / span > setpoint, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 6< / span >
setpoint = setpoint';
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Position of the sample in the frame fixed to the Granite< / span > < / span >
P_granite = < span class = "org-rainbow-delimiters-depth-1" > [< / span > pos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Position [m]< / span >
R_granite = < span class = "org-rainbow-delimiters-depth-1" > [< / span > setpoint< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Reference [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Transformation matrices of the stages< / span > < / span >
< span class = "org-comment" > % T-y< / span >
TMty = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ty ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % R-y< / span >
TMry = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % R-z< / span >
TMrz = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute Point coordinates in the new reference fixed to the NASS base< / span > < / span >
< span class = "org-comment" > % P_nass = TMrz*TMry*TMty*P_granite;< / span >
P_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > P_granite;
R_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > R_granite;
dx = R_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > P_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
dy = R_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > P_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
dz = R_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > P_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute new basis vectors linked to the NASS base< / span > < / span >
% ux_nass = TMrz< span class = "org-type" > *< / span > TMry< span class = "org-type" > *< / span > TMty< span class = "org-type" > *< / span > [< span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ];
% ux_nass = ux_nass(< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > );
% uy_nass = TMrz< span class = "org-type" > *< / span > TMry< span class = "org-type" > *< / span > TMty< span class = "org-type" > *< / span > [< span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ];
% uy_nass = uy_nass(< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > );
% uz_nass = TMrz< span class = "org-type" > *< / span > TMry< span class = "org-type" > *< / span > TMty< span class = "org-type" > *< / span > [< span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ];
% uz_nass = uz_nass(< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > );
ux_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ux_nass = ux_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
uy_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
uy_nass = uy_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
uz_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
uz_nass = uz_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Rotations error w.r.t. granite Frame< / span > < / span >
< span class = "org-comment" > % Rotations error w.r.t. granite Frame< / span >
rx_nass = pos< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry_nass = pos< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz_nass = pos< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % Rotation matrices of the Sample w.r.t. the Granite< / span >
Mrx_error = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Mry_error = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Mrz_error = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % Rotation matrix of the Sample w.r.t. the Granite< / span >
Mr_error = Mrz_error< span class = "org-type" > *< / span > Mry_error< span class = "org-type" > *< / span > Mrx_error;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Use matrix to solve< / span > < / span >
R = Mr_error< span class = "org-type" > /< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > ux_nass, uy_nass, uz_nass< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Rotation matrix from NASS base to Sample< / span >
< span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = RM2angle< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
error_nass = < span class = "org-rainbow-delimiters-depth-1" > [< / span > dx; dy; dz; thetax; thetay; thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > thetax, thetay, thetaz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > RM2angle< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > R< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > abs< span class = "org-rainbow-delimiters-depth-2" > (< / span > R< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > > < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 < span class = "org-type" > !< / span > = < span class = "org-highlight-numbers-number" > 1< / span > and R31 < span class = "org-type" > !< / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-type" > -< / span > asin< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % thetaybis = pi-thetay;< / span >
thetax = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
% thetaxbis = atan2(R(< span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > )< span class = "org-type" > /< / span > cos(thetaybis), R(< span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > )< span class = "org-type" > /< / span > cos(thetaybis));
thetaz = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
% thetazbis = atan2(R(< span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > )< span class = "org-type" > /< / span > cos(thetaybis), R(< span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 1< / span > )< span class = "org-type" > /< / span > cos(thetaybis));
< span class = "org-keyword" > else< / span >
thetaz = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
thetay = < span class = "org-type" > -< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = < span class = "org-type" > -< / span > thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-orgff6efdf" class = "outline-3" >
< h3 id = "orgff6efdf" > < span class = "section-number-3" > 6.4< / span > generateDiagPidControl< / h3 >
< div class = "outline-text-3" id = "text-6-4" >
< p >
< a id = "org9cb1fdc" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/generateDiagPidControl.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > K< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > generateDiagPidControl< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > G< / span > , < span class = "org-variable-name" > fs< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
pid_opts = pidtuneOptions< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'PhaseMargin'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'DesignFocus', 'disturbance-rejection'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
K = tf< span class = "org-rainbow-delimiters-depth-1" > (< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
input_name = G.InputName< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
output_name = G.OutputName< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
K< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > , < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = tf< span class = "org-rainbow-delimiters-depth-1" > (< / span > pidtune< span class = "org-rainbow-delimiters-depth-2" > (< / span > minreal< span class = "org-rainbow-delimiters-depth-3" > (< / span > G< span class = "org-rainbow-delimiters-depth-4" > (< / span > output_name, input_name< span class = "org-rainbow-delimiters-depth-4" > )< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > , < span class = "org-string" > 'PIDF'< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > fs, pid_opts< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
K.InputName = G.OutputName;
K.OutputName = G.InputName;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org00bd715" class = "outline-3" >
< h3 id = "org00bd715" > < span class = "section-number-3" > 6.5< / span > identifyPlant< / h3 >
< div class = "outline-text-3" id = "text-6-5" >
< p >
< a id = "org09a6cac" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/identifyPlant.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > sys< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > identifyPlant< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Options for Linearized< / span > < / span >
options = linearizeOptions;
options.SampleTime = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Name of the Simulink File< / span > < / span >
mdl = < span class = "org-string" > 'sim_nano_station_id'< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Input/Output definition< / span > < / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Fn'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'input'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Cartesian forces applied by NASS< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Dw'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'input'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Ground Motion< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Fs'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'input'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % External forces on the sample< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Fnl'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'input'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Forces applied on the NASS's legs< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Dsm'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'output'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Displacement of the sample< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Fnlm'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'output'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Force sensor in NASS's legs< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Dnlm'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'output'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Displacement of NASS's legs< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 8< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, '< span class = "org-type" > /< / span > Es'< span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , 'output'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Position Error w.r.t. NASS base< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the linearization< / span > < / span >
G = linearize< span class = "org-rainbow-delimiters-depth-1" > (< / span > mdl, io, < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
G.InputName = < span class = "org-rainbow-delimiters-depth-1" > {< / span > < span class = "org-string" > 'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dgx', 'Dgy', 'Dgz'< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'< / span > < span class = "org-rainbow-delimiters-depth-1" > }< / span > ;
G.OutputName = < span class = "org-rainbow-delimiters-depth-1" > {< / span > < span class = "org-string" > 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'< / span > < span class = "org-rainbow-delimiters-depth-1" > }< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Create the sub transfer functions< / span > < / span >
< span class = "org-comment" > % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame< / span >
sys.G_cart = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < / span > < span class = "org-string" > , < / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < / span > < span class = "org-string" > 'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From ground motion to Sample displacement< / span >
sys.G_gm = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < / span > < span class = "org-string" > , < / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < / span > < span class = "org-string" > 'Dgx', 'Dgy', 'Dgz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From direct forces applied on the sample to displacement of the sample< / span >
sys.G_fs = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < / span > < span class = "org-string" > , < / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < / span > < span class = "org-string" > 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From forces applied on NASS's legs to force sensor in each leg< / span >
sys.G_iff = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < / span > < span class = "org-string" > , < / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < / span > < span class = "org-string" > 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From forces applied on NASS's legs to displacement of each leg< / span >
sys.G_dleg = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < / span > < span class = "org-string" > , < / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < / span > < span class = "org-string" > 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From forces applied on NASS's legs to displacement of each leg< / span >
sys.G_plant = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < / span > < span class = "org-string" > , < / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < / span > < span class = "org-string" > 'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org618b4fa" class = "outline-3" >
< h3 id = "org618b4fa" > < span class = "section-number-3" > 6.6< / span > runSimulation< / h3 >
< div class = "outline-text-3" id = "text-6-6" >
< p >
< a id = "org20432cf" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/runSimulation.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > []< / span > < / span > = < span class = "org-function-name" > runSimulation< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > sys_name< / span > , < span class = "org-variable-name" > sys_mass< / span > , < span class = "org-variable-name" > ctrl_type< / span > , < span class = "org-variable-name" > act_damp< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Load the controller and save it for the simulation< / span > < / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > ctrl_type, < span class = "org-string" > 'cl'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-string" > & & strcmp< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-string" > act_damp, 'none'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
K_obj = load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/K_fb.mat'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
K = K_obj.< span class = "org-rainbow-delimiters-depth-1" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'K_%s_%s'< / span > , sys_mass, sys_name)); < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-string" > './mat/controllers.mat', 'K'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-3" > (< / span > ctrl_type, < span class = "org-string" > 'cl'< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < / span > < span class = "org-string" > & & strcmp< / span > < span class = "org-string" > < span class = "org-rainbow-delimiters-depth-3" > (< / span > < / span > < span class = "org-string" > act_damp, 'iff'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span >
K_obj = load< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-string" > './mat/K_fb_iff.mat'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > ;
K = K_obj.< span class = "org-rainbow-delimiters-depth-3" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-4" > (< / span > < span class = "org-string" > 'K_%s_%s_iff'< / span > , sys_mass, sys_name)); < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/controllers.mat', 'K'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-5" > (< / span > ctrl_type, < span class = "org-string" > 'ol'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span >
K = tf< span class = "org-rainbow-delimiters-depth-5" > (< / span > zeros< span class = "org-rainbow-delimiters-depth-6" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-6" > )< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/controllers.mat', 'K'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > 'ctrl_type should be cl or ol'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Active Damping< / span > < / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-5" > (< / span > act_damp, < span class = "org-string" > 'iff'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span >
K_iff_crit = load< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/K_iff_crit.mat'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
K_iff = K_iff_crit.< span class = "org-rainbow-delimiters-depth-5" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-6" > (< / span > < span class = "org-string" > 'K_iff_%s_%s'< / span > , sys_mass, sys_name)); < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > './mat/controllers.mat', 'K_iff', '-append'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > act_damp, < span class = "org-string" > 'none'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
K_iff = tf< span class = "org-rainbow-delimiters-depth-7" > (< / span > zeros< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > './mat/controllers.mat', 'K_iff', '-append'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_name, < span class = "org-string" > 'pz'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeNanoHexapod< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'actuator', 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_name, < span class = "org-string" > 'vc'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeNanoHexapod< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'actuator', 'lorentz'< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sys_name should be pz or vc'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_mass, < span class = "org-string" > 'light'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeSample< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'mass'< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_mass, < span class = "org-string" > 'heavy'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeSample< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'mass'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sys_mass should be light or heavy'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the simulation< / span > < / span >
< span class = "org-matlab-simulink-keyword" > sim< / span > < span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sim_nano_station_ctrl.slx'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Split the Dsample matrix into vectors< / span > < / span >
< span class = "org-rainbow-delimiters-depth-7" > [< / span > Dx, Dy, Dz, Rx, Ry, Rz< span class = "org-rainbow-delimiters-depth-7" > ]< / span > = matSplit< span class = "org-rainbow-delimiters-depth-7" > (< / span > Es.Data, < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
time = Dsample.Time; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save the result< / span > < / span >
filename = sprintf< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sim_%s_%s_%s_%s'< / span > , sys_mass, sys_name, ctrl_type, act_damp);
save< span class = "org-rainbow-delimiters-depth-8" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-9" > (< / span > < span class = "org-string" > './mat/%s.mat'< / span > , filename), < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K'< / span > < span class = "org-rainbow-delimiters-depth-9" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< / div >
< div id = "outline-container-org0cf6746" class = "outline-2" >
< h2 id = "org0cf6746" > < span class = "section-number-2" > 7< / span > Initialize Elements< / h2 >
< div class = "outline-text-2" id = "text-7" >
< / div >
< div id = "outline-container-org25ddf30" class = "outline-3" >
< h3 id = "org25ddf30" > < span class = "section-number-3" > 7.1< / span > Simulation Configuration< / h3 >
< div class = "outline-text-3" id = "text-7-1" >
< p >
< a id = "orga7027a0" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeSimConf.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > []< / span > < / span > = < span class = "org-function-name" > initializeSimConf< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Ts'< / span > , < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 4< / span > , < span class = "org-underline" > ...< / span > < span class = "org-comment" > % Sampling time [s]< / span >
< span class = "org-string" > 'Tsim'< / span > , < span class = "org-highlight-numbers-number" > 10< / span > , < span class = "org-underline" > ...< / span > < span class = "org-comment" > % Simulation time [s]< / span >
< span class = "org-string" > 'cl_time'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , < span class = "org-underline" > ...< / span > < span class = "org-comment" > % Close Loop time [s]< / span >
< span class = "org-string" > 'gravity'< / span > , false < span class = "org-underline" > ...< / span > < span class = "org-comment" > % Gravity along the z axis< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
sim_conf = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
sim_conf.Ts = opts.Ts;
sim_conf.Tsim = opts.Tsim;
sim_conf.cl_time = opts.cl_time;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Gravity< / span > < / span >
< span class = "org-keyword" > if< / span > opts.gravity
sim_conf.g = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 9< / span > .< span class = "org-highlight-numbers-number" > 8< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
< span class = "org-keyword" > else< / span >
sim_conf.g = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/sim_conf.mat', 'sim_conf'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org4ab253c" class = "outline-3" >
< h3 id = "org4ab253c" > < span class = "section-number-3" > 7.2< / span > Experiment< / h3 >
< div class = "outline-text-3" id = "text-7-2" >
< p >
< a id = "orga068a4d" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeExperiment.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > []< / span > < / span > = < span class = "org-function-name" > initializeExperiment< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > exp_name< / span > , < span class = "org-variable-name" > sys_mass< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > exp_name, < span class = "org-string" > 'tomography'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
opts_sim = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Tsim'< / span > , < span class = "org-highlight-numbers-number" > 5< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'cl_time'< / span > , < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-underline" > ...< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
initializeSimConf< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts_sim< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_mass, < span class = "org-string" > 'light'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
opts_inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dw'< / span > , true, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Rz'< / span > , < span class = "org-highlight-numbers-number" > 60< / span > < span class = "org-underline" > ...< / span > < span class = "org-comment" > % rpm< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcpm< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_mass, < span class = "org-string" > 'heavy'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
opts_inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dw'< / span > , true, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Rz'< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-underline" > ...< / span > < span class = "org-comment" > % rpm< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'sys_mass should be light or heavy'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
initializeInputs< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts_inputs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'exp_name is only configured for tomography'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org410da45" class = "outline-3" >
< h3 id = "org410da45" > < span class = "section-number-3" > 7.3< / span > Inputs< / h3 >
< div class = "outline-text-3" id = "text-7-3" >
< p >
< a id = "orge833d8b" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeInputs.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > inputs< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeInputs< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dw'< / span > , false, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dy'< / span > , false, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Ry'< / span > , false, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Rz'< / span > , false, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dh'< / span > , false, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Rm'< / span > , false, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dn'< / span > , false < span class = "org-underline" > ...< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Load Sampling Time and Simulation Time< / span > < / span >
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/sim_conf.mat', 'sim_conf'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Define the time vector< / span > < / span >
t = < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-type" > :< / span > sim_conf.Ts< span class = "org-type" > :< / span > sim_conf.Tsim;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ground motion - Dw< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dw< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dw < span class = "org-type" > ==< / span > true
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/perturbations.mat', 'Wxg'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 100< / span > < span class = "org-type" > *< / span > random< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'norm'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = lsim< span class = "org-rainbow-delimiters-depth-1" > (< / span > Wxg, Dw< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = lsim< span class = "org-rainbow-delimiters-depth-1" > (< / span > Wxg, Dw< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = lsim< span class = "org-rainbow-delimiters-depth-1" > (< / span > Wxg, Dw< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dw< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dw < span class = "org-type" > ==< / span > false
Dw = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dw = opts.Dw;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Translation stage - Dy< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dy< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dy < span class = "org-type" > ==< / span > true
Dy = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dy< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dy < span class = "org-type" > ==< / span > false
Dy = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dy = opts.Dy;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Tilt Stage - Ry< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Ry < span class = "org-type" > ==< / span > true
Ry = < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 360< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Ry < span class = "org-type" > ==< / span > false
Ry = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > isnumeric< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > length< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 1< / span >
Ry = opts.Ry< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 360< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > ones< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Ry = opts.Ry;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Spindle - Rz< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Rz < span class = "org-type" > ==< / span > true
Rz = < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-type" > *< / span > t;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Rz < span class = "org-type" > ==< / span > false
Rz = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > isnumeric< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > length< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 1< / span >
Rz = opts.Rz< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 60< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > t;
< span class = "org-keyword" > else< / span >
Rz = opts.Rz;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Micro Hexapod - Dh< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dh< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dh < span class = "org-type" > ==< / span > true
Dh = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dh< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dh < span class = "org-type" > ==< / span > false
Dh = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dh = opts.Dh;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Axis Compensation - Rm< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rm< span class = "org-rainbow-delimiters-depth-1" > )< / span >
Rm = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Rm< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > ones< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Rm = opts.Rm;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Nano Hexapod - Dn< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dn< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dn < span class = "org-type" > ==< / span > true
Dn = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dn< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dn < span class = "org-type" > ==< / span > false
Dn = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dn = opts.Dn;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Setpoint - Ds< / span > < / span >
Ds = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :length< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > t< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
Ds< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > , < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = computeSetpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > Dy< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , Ry< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , Rz< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% External Forces applied on the Granite< / span > < / span >
Fg = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% External Forces applied on the Sample< / span > < / span >
Fs = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Create the input Structure that will contain all the inputs< / span > < / span >
inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Ts'< / span > , sim_conf.Ts, < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dw'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dw, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dy'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dy, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Ry'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Ry, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Rz'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Rz, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dh'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dh, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Rm'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Rm, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Dn'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dn, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Ds'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Ds, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Fg'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Fg, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'Fs'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Fs, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-underline" > ...< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/inputs.mat', 'inputs'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > setpoint< / span > = < span class = "org-function-name" > computeSetpoint< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > ty< / span > , < span class = "org-variable-name" > ry< / span > , < span class = "org-variable-name" > rz< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
setpoint = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ty< / span > < / span >
TMTy = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ty ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ry< / span > < / span >
TMRy = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Rz< / span > < / span >
TMRz = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% All stages< / span > < / span >
TM = TMTy< span class = "org-type" > *< / span > TMRy< span class = "org-type" > *< / span > TMRz;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = RM2angle< span class = "org-rainbow-delimiters-depth-1" > (< / span > TM< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = TM< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > thetax, thetay, thetaz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > RM2angle< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > R< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > abs< span class = "org-rainbow-delimiters-depth-2" > (< / span > R< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > > < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 < span class = "org-type" > !< / span > = < span class = "org-highlight-numbers-number" > 1< / span > and R31 < span class = "org-type" > !< / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-type" > -< / span > asin< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetax = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetaz = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
thetaz = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
thetay = < span class = "org-type" > -< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = < span class = "org-type" > -< / span > thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org05c393c" class = "outline-3" >
< h3 id = "org05c393c" > < span class = "section-number-3" > 7.4< / span > Ground< / h3 >
< div class = "outline-text-3" id = "text-7-4" >
< p >
< a id = "org2c2d770" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeGround.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > ground< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeGround< / span > < span class = "org-rainbow-delimiters-depth-1" > ()< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
ground = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
ground.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [m]< / span >
ground.density = < span class = "org-highlight-numbers-number" > 2800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ground.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'ground', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-orgd3a8362" class = "outline-3" >
< h3 id = "orgd3a8362" > < span class = "section-number-3" > 7.5< / span > Granite< / h3 >
< div class = "outline-text-3" id = "text-7-5" >
< p >
< a id = "org88802ab" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeGranite.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > granite< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeGranite< / span > < span class = "org-rainbow-delimiters-depth-1" > ()< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
granite = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Static Properties< / span > < / span >
granite.density = < span class = "org-highlight-numbers-number" > 2800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
granite.volume = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 72< / span > ; < span class = "org-comment" > % [m3] TODO - should< / span >
granite.mass = granite.density< span class = "org-type" > *< / span > granite.volume; < span class = "org-comment" > % [kg]< / span >
granite.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
granite.STEP = < span class = "org-string" > './STEPS/granite/granite.STEP'< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Dynamical Properties< / span > < / span >
granite.k.x = < span class = "org-highlight-numbers-number" > 1e8< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.c.x = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
granite.k.y = < span class = "org-highlight-numbers-number" > 1e8< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.c.y = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
granite.k.z = < span class = "org-highlight-numbers-number" > 1e8< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.c.z = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Positioning parameters< / span > < / span >
granite.sample_pos = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 8< / span > ; < span class = "org-comment" > % Z-offset for the initial position of the sample [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'granite', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org534ce7a" class = "outline-3" >
< h3 id = "org534ce7a" > < span class = "section-number-3" > 7.6< / span > Translation Stage< / h3 >
< div class = "outline-text-3" id = "text-7-6" >
< p >
< a id = "org6c15a5d" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeTy.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > ty< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeTy< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , false< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
ty = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Y-Translation - Static Properties< / span > < / span >
< span class = "org-comment" > % Ty Granite frame< / span >
ty.granite_frame.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.granite_frame.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.granite_frame.STEP = < span class = "org-string" > './STEPS/Ty/Ty_Granite_Frame.STEP'< / span > ;
< span class = "org-comment" > % Guide Translation Ty< / span >
ty.guide.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide.STEP = < span class = "org-string" > './STEPS/ty/Ty_Guide.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation12< / span >
ty.guide12.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide12.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide12.STEP = < span class = "org-string" > './STEPS/Ty/Ty_Guide_12.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation11< / span >
ty.guide11.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide11.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide11.STEP = < span class = "org-string" > './STEPS/ty/Ty_Guide_11.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation22< / span >
ty.guide22.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide22.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide22.STEP = < span class = "org-string" > './STEPS/ty/Ty_Guide_22.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation21< / span >
ty.guide21.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide21.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide21.STEP = < span class = "org-string" > './STEPS/Ty/Ty_Guide_21.STEP'< / span > ;
< span class = "org-comment" > % Ty - Plateau translation< / span >
ty.frame.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.frame.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.frame.STEP = < span class = "org-string" > './STEPS/ty/Ty_Stage.STEP'< / span > ;
< span class = "org-comment" > % Ty Stator Part< / span >
ty.stator.density = < span class = "org-highlight-numbers-number" > 5400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.stator.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.stator.STEP = < span class = "org-string" > './STEPS/ty/Ty_Motor_Stator.STEP'< / span > ;
< span class = "org-comment" > % Ty Rotor Part< / span >
ty.rotor.density = < span class = "org-highlight-numbers-number" > 5400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.rotor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.rotor.STEP = < span class = "org-string" > './STEPS/ty/Ty_Motor_Rotor.STEP'< / span > ;
ty.m = < span class = "org-highlight-numbers-number" > 250< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Y-Translation - Dynamicals Properties< / span > < / span >
< span class = "org-keyword" > if< / span > opts.rigid
ty.k.ax = < span class = "org-highlight-numbers-number" > 1e10< / span > ; % Axial Stiffness < span class = "org-keyword" > for< / span > each of the < span class = "org-highlight-numbers-number" > 4< / span > guidance (y) [N< span class = "org-type" > /< / span > m]
< span class = "org-keyword" > else< / span >
ty.k.ax = < span class = "org-highlight-numbers-number" > 1e7< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 4< / span > ; % Axial Stiffness < span class = "org-keyword" > for< / span > each of the < span class = "org-highlight-numbers-number" > 4< / span > guidance (y) [N< span class = "org-type" > /< / span > m]
< span class = "org-keyword" > end< / span >
ty.k.rad = < span class = "org-highlight-numbers-number" > 9e9< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 4< / span > ; % Radial Stiffness < span class = "org-keyword" > for< / span > each of the < span class = "org-highlight-numbers-number" > 4< / span > guidance (x< span class = "org-type" > -< / span > z) [N< span class = "org-type" > /< / span > m]
ty.c.ax = < span class = "org-highlight-numbers-number" > 100< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ty.k.ax< span class = "org-type" > /< / span > ty.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ty.c.rad = < span class = "org-highlight-numbers-number" > 100< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ty.k.rad< span class = "org-type" > /< / span > ty.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'ty', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-orgf227dfc" class = "outline-3" >
< h3 id = "orgf227dfc" > < span class = "section-number-3" > 7.7< / span > Tilt Stage< / h3 >
< div class = "outline-text-3" id = "text-7-7" >
< p >
< a id = "orga9f9fb6" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeRy.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > ry< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeRy< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , false< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
ry = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Tilt Stage - Static Properties< / span > < / span >
< span class = "org-comment" > % Ry - Guide for the tilt stage< / span >
ry.guide.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.guide.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.guide.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Guide.STEP'< / span > ;
< span class = "org-comment" > % Ry - Rotor of the motor< / span >
ry.rotor.density = < span class = "org-highlight-numbers-number" > 2400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.rotor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.rotor.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Motor_Axis.STEP'< / span > ;
< span class = "org-comment" > % Ry - Motor< / span >
ry.motor.density = < span class = "org-highlight-numbers-number" > 3200< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.motor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.motor.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Motor.STEP'< / span > ;
< span class = "org-comment" > % Ry - Plateau Tilt< / span >
ry.stage.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.stage.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.stage.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Stage.STEP'< / span > ;
ry.m = < span class = "org-highlight-numbers-number" > 200< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Tilt Stage - Dynamical Properties< / span > < / span >
< span class = "org-keyword" > if< / span > opts.rigid
ry.k.tilt = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % Rotation stiffness around y [N*m/deg]< / span >
< span class = "org-keyword" > else< / span >
ry.k.tilt = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % Rotation stiffness around y [N*m/deg]< / span >
< span class = "org-keyword" > end< / span >
ry.k.h = < span class = "org-highlight-numbers-number" > 357e6< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 4< / span > ; < span class = "org-comment" > % Stiffness in the direction of the guidance [N/m]< / span >
ry.k.rad = < span class = "org-highlight-numbers-number" > 555e6< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 4< / span > ; < span class = "org-comment" > % Stiffness in the top direction [N/m]< / span >
ry.k.rrad = < span class = "org-highlight-numbers-number" > 238e6< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 4< / span > ; < span class = "org-comment" > % Stiffness in the side direction [N/m]< / span >
ry.c.h = < span class = "org-highlight-numbers-number" > 10< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.h< span class = "org-type" > /< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry.c.rad = < span class = "org-highlight-numbers-number" > 10< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.rad< span class = "org-type" > /< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry.c.rrad = < span class = "org-highlight-numbers-number" > 10< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.rrad< span class = "org-type" > /< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry.c.tilt = < span class = "org-highlight-numbers-number" > 10< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.tilt< span class = "org-type" > /< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Positioning parameters< / span > < / span >
ry.z_offset = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 58178< / span > ; < span class = "org-comment" > % Z-Offset so that the center of rotation matches the sample center [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'ry', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-orge76075d" class = "outline-3" >
< h3 id = "orge76075d" > < span class = "section-number-3" > 7.8< / span > Spindle< / h3 >
< div class = "outline-text-3" id = "text-7-8" >
< p >
< a id = "org9261d64" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeRz.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > rz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeRz< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , false< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
rz = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Spindle - Static Properties< / span > < / span >
< span class = "org-comment" > % Spindle - Slip Ring< / span >
rz.slipring.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
rz.slipring.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
rz.slipring.STEP = < span class = "org-string" > './STEPS/rz/Spindle_Slip_Ring.STEP'< / span > ;
< span class = "org-comment" > % Spindle - Rotor< / span >
rz.rotor.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
rz.rotor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
rz.rotor.STEP = < span class = "org-string" > './STEPS/rz/Spindle_Rotor.STEP'< / span > ;
< span class = "org-comment" > % Spindle - Stator< / span >
rz.stator.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
rz.stator.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
rz.stator.STEP = < span class = "org-string" > './STEPS/rz/Spindle_Stator.STEP'< / span > ;
< span class = "org-comment" > % Estimated mass of the mooving part< / span >
rz.m = < span class = "org-highlight-numbers-number" > 250< / span > ; < span class = "org-comment" > % [kg]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Spindle - Dynamical Properties< / span > < / span >
< span class = "org-comment" > % Estimated stiffnesses< / span >
rz.k.ax = < span class = "org-highlight-numbers-number" > 2e9< / span > ; < span class = "org-comment" > % Axial Stiffness [N/m]< / span >
rz.k.rad = < span class = "org-highlight-numbers-number" > 7e8< / span > ; < span class = "org-comment" > % Radial Stiffness [N/m]< / span >
rz.k.rot = < span class = "org-highlight-numbers-number" > 100e6< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 360< / span > ; < span class = "org-comment" > % Rotational Stiffness [N*m/deg]< / span >
< span class = "org-keyword" > if< / span > opts.rigid
rz.k.tilt = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % Vertical Rotational Stiffness [N*m/deg]< / span >
< span class = "org-keyword" > else< / span >
rz.k.tilt = < span class = "org-highlight-numbers-number" > 1e2< / span > ; < span class = "org-comment" > % TODO what value should I put? [N*m/deg]< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-comment" > % TODO< / span >
rz.c.ax = < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.ax< span class = "org-type" > /< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz.c.rad = < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.rad< span class = "org-type" > /< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz.c.tilt = < span class = "org-highlight-numbers-number" > 100< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.tilt< span class = "org-type" > /< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz.c.rot = < span class = "org-highlight-numbers-number" > 100< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.rot< span class = "org-type" > /< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'rz', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org27dbc96" class = "outline-3" >
< h3 id = "org27dbc96" > < span class = "section-number-3" > 7.9< / span > Micro Hexapod< / h3 >
< div class = "outline-text-3" id = "text-7-9" >
< p >
< a id = "org8838bd4" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeMicroHexapod.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > micro_hexapod< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeMicroHexapod< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Stewart Object< / span > < / span >
micro_hexapod = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
micro_hexapod.h = < span class = "org-highlight-numbers-number" > 350< / span > ; < span class = "org-comment" > % Total height of the platform [mm]< / span >
% micro_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 269< / span > .< span class = "org-highlight-numbers-number" > 26< / span > ; < span class = "org-comment" > % Distance from the top platform to the Jacobian point [mm]< / span >
micro_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 270< / span > ; < span class = "org-comment" > % Distance from the top platform to the Jacobian point [mm]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Bottom Plate - Mechanical Design< / span > < / span >
BP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
BP.rad.int = < span class = "org-highlight-numbers-number" > 110< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
BP.rad.ext = < span class = "org-highlight-numbers-number" > 207< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % External Radius [mm]< / span >
BP.thickness = < span class = "org-highlight-numbers-number" > 26< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
BP.leg.rad = < span class = "org-highlight-numbers-number" > 175< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
BP.leg.ang = < span class = "org-highlight-numbers-number" > 9< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
BP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
BP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
BP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > BP.rad.int BP.thickness; BP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext BP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Top Plate - Mechanical Design< / span > < / span >
TP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
TP.rad.int = < span class = "org-highlight-numbers-number" > 82< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.rad.ext = < span class = "org-highlight-numbers-number" > 150< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.thickness = < span class = "org-highlight-numbers-number" > 26< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
TP.leg.rad = < span class = "org-highlight-numbers-number" > 118< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
TP.leg.ang = < span class = "org-highlight-numbers-number" > 12< / span > .< span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
TP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
TP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
TP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > TP.rad.int TP.thickness; TP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext TP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Struts< / span > < / span >
Leg = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
Leg.stroke = < span class = "org-highlight-numbers-number" > 10e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > ; < span class = "org-comment" > % Maximum Stroke of each leg [m]< / span >
Leg.k.ax = < span class = "org-highlight-numbers-number" > 5e7< / span > ; < span class = "org-comment" > % Stiffness of each leg [N/m]< / span >
Leg.ksi.ax = < span class = "org-highlight-numbers-number" > 3< / span > ; < span class = "org-comment" > % Maximum amplification at resonance []< / span >
Leg.rad.bottom = < span class = "org-highlight-numbers-number" > 25< / span > ; < span class = "org-comment" > % Radius of the cylinder of the bottom part [mm]< / span >
Leg.rad.top = < span class = "org-highlight-numbers-number" > 17< / span > ; < span class = "org-comment" > % Radius of the cylinder of the top part [mm]< / span >
Leg.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
Leg.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.sphere.bottom = Leg.rad.bottom; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.sphere.top = Leg.rad.top; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.m = TP.density< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.ext< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.int< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
Leg = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > Leg< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Sphere< / span > < / span >
SP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
SP.height.bottom = < span class = "org-highlight-numbers-number" > 27< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.height.top = < span class = "org-highlight-numbers-number" > 27< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.density.bottom = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.density.top = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.k.ax = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
SP.ksi.ax = < span class = "org-highlight-numbers-number" > 10< / span > ;
SP.thickness.bottom = SP.height.bottom< span class = "org-type" > -< / span > Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.thickness.top = SP.height.top< span class = "org-type" > -< / span > Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.rad.bottom = Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.rad.top = Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.m = SP.density.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > SP.rad.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > SP.height.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
SP = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > SP< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
Leg.support.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.bottom; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom SP.height.bottom< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Leg.support.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.top; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top SP.height.top< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
micro_hexapod.BP = BP;
micro_hexapod.TP = TP;
micro_hexapod.Leg = Leg;
micro_hexapod.SP = SP;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
micro_hexapod = initializeParameters< span class = "org-rainbow-delimiters-depth-1" > (< / span > micro_hexapod< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'micro_hexapod', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > element< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > updateDamping< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > element< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
field = fieldnames< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :length< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > field< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
element.c.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > element.ksi.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k.< span class = "org-rainbow-delimiters-depth-2" > (< / span > field< span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > element.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeParameters< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Connection points on base and top plate w.r.t. World frame at the center of the base plate< / span > < / span >
stewart.pos_base = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
stewart.pos_top = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
alpha_b = stewart.BP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % angle de dé calage par rapport à < span class = "org-highlight-numbers-number" > 120< / span > deg (pour positionner les supports bases)
alpha_t = stewart.TP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % < span class = "org-type" > +-< / span > offset angle from < span class = "org-highlight-numbers-number" > 120< / span > degree spacing on top
height = < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.h< span class = "org-type" > -< / span > stewart.BP.thickness< span class = "org-type" > -< / span > stewart.TP.thickness< span class = "org-type" > -< / span > stewart.Leg.sphere.bottom< span class = "org-type" > -< / span > stewart.Leg.sphere.top< span class = "org-type" > -< / span > stewart.SP.thickness.bottom< span class = "org-type" > -< / span > stewart.SP.thickness.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % TODO< / span >
radius_b = stewart.BP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % rayon emplacement support base< / span >
radius_t = stewart.TP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % top radius in meters< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 3< / span > < / span >
< span class = "org-comment" > % base points< / span >
angle_m_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_b;
angle_p_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_b;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % top points< / span >
% Top points are < span class = "org-highlight-numbers-number" > 60< / span > degrees offset
angle_m_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
angle_p_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-comment" > % permute pos_top points so that legs are end points of base and top points< / span >
stewart.pos_top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 5< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; %< span class = "org-highlight-numbers-number" > 6th< / span > point on top connects to < span class = "org-highlight-numbers-number" > 1st< / span > on bottom
stewart.pos_top_tranform = stewart.pos_top < span class = "org-type" > -< / span > height< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% leg vectors< / span > < / span >
legs = stewart.pos_top < span class = "org-type" > -< / span > stewart.pos_base;
leg_length = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > legs< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = legs< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
stewart.Leg.lenght = < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > *< / span > leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ;
stewart.Leg.shape.bot = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.bottom stewart.Leg.lenght; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.top stewart.Leg.lenght; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.top < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght; < span class = "org-underline" > ...< / span >
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Calculate revolute and cylindrical axes< / span > < / span >
rev1 = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2 = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
cyl1 = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
rev1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-rainbow-delimiters-depth-2" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = rev1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-type" > -< / span > cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = rev2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
cyl1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = leg_vectors< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Coordinate systems< / span > < / span >
stewart.lower_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
stewart.upper_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
stewart.lower_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > '< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.upper_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > '< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Position Matrix< / span > < / span >
stewart.M_pos_base = stewart.pos_base < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > height< span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > stewart.TP.thickness< span class = "org-type" > +< / span > stewart.Leg.sphere.top< span class = "org-type" > +< / span > stewart.SP.thickness.top< span class = "org-type" > +< / span > stewart.jacobian< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute Jacobian Matrix< / span > < / span >
aa = stewart.pos_top_tranform < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.jacobian < span class = "org-type" > -< / span > stewart.TP.thickness < span class = "org-type" > -< / span > stewart.SP.height.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.J = getJacobianMatrix< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors', aa'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > J< / span > = < span class = "org-function-name" > getJacobianMatrix< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > RM< / span > ,< span class = "org-variable-name" > M_pos_base< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
% RM< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] unit vector of each leg in the fixed frame
% M_pos_base< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] vector of the leg connection at the top platform location in the fixed frame
J = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = RM';
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > M_pos_base, RM< span class = "org-rainbow-delimiters-depth-1" > )< / span > ';
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org4e5c6e0" class = "outline-3" >
< h3 id = "org4e5c6e0" > < span class = "section-number-3" > 7.10< / span > Center of gravity compensation< / h3 >
< div class = "outline-text-3" id = "text-7-10" >
< p >
< a id = "orgfa0c2ac" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeAxisc.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > axisc< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeAxisc< / span > < span class = "org-rainbow-delimiters-depth-1" > ()< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
axisc = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Axis Compensator - Static Properties< / span > < / span >
< span class = "org-comment" > % Structure< / span >
axisc.structure.density = < span class = "org-highlight-numbers-number" > 3400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.structure.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.structure.STEP = < span class = "org-string" > './STEPS/axisc/axisc_structure.STEP'< / span > ;
< span class = "org-comment" > % Wheel< / span >
axisc.wheel.density = < span class = "org-highlight-numbers-number" > 2700< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.wheel.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.wheel.STEP = < span class = "org-string" > './STEPS/axisc/axisc_wheel.STEP'< / span > ;
< span class = "org-comment" > % Mass< / span >
axisc.mass.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.mass.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.mass.STEP = < span class = "org-string" > './STEPS/axisc/axisc_mass.STEP'< / span > ;
< span class = "org-comment" > % Gear< / span >
axisc.gear.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.gear.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.gear.STEP = < span class = "org-string" > './STEPS/axisc/axisc_gear.STEP'< / span > ;
axisc.m = < span class = "org-highlight-numbers-number" > 40< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Axis Compensator - Dynamical Properties< / span > < / span >
axisc.k.ax = < span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-comment" > % TODO [N*m/deg)]< / span >
axisc.c.ax = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > axisc.k.ax< span class = "org-type" > /< / span > axisc.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'axisc', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-orgc5a4039" class = "outline-3" >
< h3 id = "orgc5a4039" > < span class = "section-number-3" > 7.11< / span > Mirror< / h3 >
< div class = "outline-text-3" id = "text-7-11" >
< p >
< a id = "orgd5386d5" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeMirror.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > []< / span > < / span > = < span class = "org-function-name" > initializeMirror< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'shape', 'spherical'< / span > , < span class = "org-underline" > ...< / span > < span class = "org-comment" > % spherical or conical< / span >
< span class = "org-string" > 'angle'< / span > , < span class = "org-highlight-numbers-number" > 45< / span > < span class = "org-underline" > ...< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
mirror = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
mirror.h = < span class = "org-highlight-numbers-number" > 50< / span > ; < span class = "org-comment" > % height of the mirror [mm]< / span >
mirror.thickness = < span class = "org-highlight-numbers-number" > 25< / span > ; < span class = "org-comment" > % Thickness of the plate supporting the sample [mm]< / span >
mirror.hole_rad = < span class = "org-highlight-numbers-number" > 120< / span > ; < span class = "org-comment" > % radius of the hole in the mirror [mm]< / span >
mirror.support_rad = < span class = "org-highlight-numbers-number" > 100< / span > ; < span class = "org-comment" > % radius of the support plate [mm]< / span >
mirror.jacobian = < span class = "org-highlight-numbers-number" > 150< / span > ; < span class = "org-comment" > % point of interest offset in z (above the top surfave) [mm]< / span >
mirror.rad = < span class = "org-highlight-numbers-number" > 180< / span > ; < span class = "org-comment" > % radius of the mirror (at the bottom surface) [mm]< / span >
mirror.density = < span class = "org-highlight-numbers-number" > 2400< / span > ; < span class = "org-comment" > % Density of the mirror [kg/m3]< / span >
mirror.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-highlight-numbers-number" > 1< / span > .< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > .< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color of the mirror< / span >
mirror.cone_length = mirror.rad< span class = "org-type" > *< / span > tand< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.angle< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > mirror.h< span class = "org-type" > +< / span > mirror.jacobian; < span class = "org-comment" > % Distance from Apex point of the cone to jacobian point< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Shape< / span > < / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-underline" > ...< / span >
< span class = "org-highlight-numbers-number" > 0< / span > mirror.h< span class = "org-type" > -< / span > mirror.thickness
mirror.hole_rad mirror.h< span class = "org-type" > -< / span > mirror.thickness; < span class = "org-underline" > ...< / span >
mirror.hole_rad < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-underline" > ...< / span >
mirror.rad < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-underline" > ...< / span >
< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.shape, < span class = "org-string" > 'spherical'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
mirror.sphere_radius = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > mirror.jacobian< span class = "org-type" > +< / span > mirror.h< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > +< / span > mirror.rad< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Radius of the sphere [mm]< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > z< / span > = < span class = "org-constant" > linspace< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 0< / span > < / span > < span class = "org-constant" > , mirror.h, < / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 101< / span > < / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > mirror.shape; sqrt< span class = "org-rainbow-delimiters-depth-2" > (< / span > mirror.sphere_radius< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > -< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > z< span class = "org-type" > -< / span > mirror.jacobian< span class = "org-type" > -< / span > mirror.h< span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > z< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.shape, < span class = "org-string" > 'conical'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > mirror.shape; mirror.rad< span class = "org-type" > +< / span > mirror.h< span class = "org-type" > /< / span > tand< span class = "org-rainbow-delimiters-depth-2" > (< / span > opts.angle< span class = "org-rainbow-delimiters-depth-2" > )< / span > mirror.h< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Shape should be either conical or spherical'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > mirror.shape; < span class = "org-highlight-numbers-number" > 0< / span > mirror.h< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'mirror', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-orgaf1e477" class = "outline-3" >
< h3 id = "orgaf1e477" > < span class = "section-number-3" > 7.12< / span > Nano Hexapod< / h3 >
< div class = "outline-text-3" id = "text-7-12" >
< p >
< a id = "org78e3789" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeNanoHexapod.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > nano_hexapod< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeNanoHexapod< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'actuator', 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
opts.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Stewart Object< / span > < / span >
nano_hexapod = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
nano_hexapod.h = < span class = "org-highlight-numbers-number" > 90< / span > ; < span class = "org-comment" > % Total height of the platform [mm]< / span >
nano_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 175< / span > ; < span class = "org-comment" > % Point where the Jacobian is computed => Center of rotation [mm]< / span >
% nano_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 174< / span > .< span class = "org-highlight-numbers-number" > 26< / span > ; < span class = "org-comment" > % Point where the Jacobian is computed => Center of rotation [mm]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Bottom Plate< / span > < / span >
BP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
BP.rad.int = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
BP.rad.ext = < span class = "org-highlight-numbers-number" > 150< / span > ; < span class = "org-comment" > % External Radius [mm]< / span >
BP.thickness = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
BP.leg.rad = < span class = "org-highlight-numbers-number" > 100< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
BP.leg.ang = < span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
BP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ;% Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
BP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
BP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > BP.rad.int BP.thickness; BP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext BP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Top Plate< / span > < / span >
TP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
TP.rad.int = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.rad.ext = < span class = "org-highlight-numbers-number" > 100< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.thickness = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
TP.leg.rad = < span class = "org-highlight-numbers-number" > 90< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
TP.leg.ang = < span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
TP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ;% Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
TP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
TP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > TP.rad.int TP.thickness; TP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext TP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Leg< / span > < / span >
Leg = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
Leg.stroke = < span class = "org-highlight-numbers-number" > 80e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > ; < span class = "org-comment" > % Maximum Stroke of each leg [m]< / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.actuator, < span class = "org-string" > 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
Leg.k.ax = < span class = "org-highlight-numbers-number" > 1e7< / span > ; < span class = "org-comment" > % Stiffness of each leg [N/m]< / span >
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.actuator, < span class = "org-string" > 'lorentz'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
Leg.k.ax = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % Stiffness of each leg [N/m]< / span >
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts.actuator should be piezo or lorentz'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
Leg.ksi.ax = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Maximum amplification at resonance []< / span >
Leg.rad.bottom = < span class = "org-highlight-numbers-number" > 12< / span > ; < span class = "org-comment" > % Radius of the cylinder of the bottom part [mm]< / span >
Leg.rad.top = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Radius of the cylinder of the top part [mm]< / span >
Leg.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
Leg.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.sphere.bottom = Leg.rad.bottom; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.sphere.top = Leg.rad.top; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.m = TP.density< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.ext< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.int< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
Leg = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > Leg< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Sphere< / span > < / span >
SP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
SP.height.bottom = < span class = "org-highlight-numbers-number" > 15< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.height.top = < span class = "org-highlight-numbers-number" > 15< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.density.bottom = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.density.top = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.k.ax = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
SP.ksi.ax = < span class = "org-highlight-numbers-number" > 3< / span > ;
SP.thickness.bottom = SP.height.bottom< span class = "org-type" > -< / span > Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.thickness.top = SP.height.top< span class = "org-type" > -< / span > Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.rad.bottom = Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.rad.top = Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.m = SP.density.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > SP.rad.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > SP.height.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
SP = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > SP< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
Leg.support.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.bottom; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom SP.height.bottom< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Leg.support.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.top; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top SP.height.top< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
nano_hexapod.BP = BP;
nano_hexapod.TP = TP;
nano_hexapod.Leg = Leg;
nano_hexapod.SP = SP;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
nano_hexapod = initializeParameters< span class = "org-rainbow-delimiters-depth-1" > (< / span > nano_hexapod< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'nano_hexapod', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > element< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > updateDamping< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > element< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
field = fieldnames< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :length< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > field< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
element.c.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > element.ksi.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k.< span class = "org-rainbow-delimiters-depth-2" > (< / span > field< span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > element.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeParameters< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Connection points on base and top plate w.r.t. World frame at the center of the base plate< / span > < / span >
stewart.pos_base = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
stewart.pos_top = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
alpha_b = stewart.BP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % angle de dé calage par rapport à < span class = "org-highlight-numbers-number" > 120< / span > deg (pour positionner les supports bases)
alpha_t = stewart.TP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % < span class = "org-type" > +-< / span > offset angle from < span class = "org-highlight-numbers-number" > 120< / span > degree spacing on top
height = < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.h< span class = "org-type" > -< / span > stewart.BP.thickness< span class = "org-type" > -< / span > stewart.TP.thickness< span class = "org-type" > -< / span > stewart.Leg.sphere.bottom< span class = "org-type" > -< / span > stewart.Leg.sphere.top< span class = "org-type" > -< / span > stewart.SP.thickness.bottom< span class = "org-type" > -< / span > stewart.SP.thickness.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % TODO< / span >
radius_b = stewart.BP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % rayon emplacement support base< / span >
radius_t = stewart.TP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % top radius in meters< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 3< / span > < / span >
< span class = "org-comment" > % base points< / span >
angle_m_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_b;
angle_p_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_b;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % top points< / span >
% Top points are < span class = "org-highlight-numbers-number" > 60< / span > degrees offset
angle_m_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
angle_p_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-comment" > % permute pos_top points so that legs are end points of base and top points< / span >
stewart.pos_top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 5< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; %< span class = "org-highlight-numbers-number" > 6th< / span > point on top connects to < span class = "org-highlight-numbers-number" > 1st< / span > on bottom
stewart.pos_top_tranform = stewart.pos_top < span class = "org-type" > -< / span > height< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% leg vectors< / span > < / span >
legs = stewart.pos_top < span class = "org-type" > -< / span > stewart.pos_base;
leg_length = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > legs< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = legs< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
stewart.Leg.lenght = < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > *< / span > leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ;
stewart.Leg.shape.bot = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.bottom stewart.Leg.lenght; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.top stewart.Leg.lenght; < span class = "org-underline" > ...< / span >
stewart.Leg.rad.top < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght; < span class = "org-underline" > ...< / span >
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Calculate revolute and cylindrical axes< / span > < / span >
rev1 = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2 = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
cyl1 = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
rev1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-rainbow-delimiters-depth-2" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = rev1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-type" > -< / span > cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = rev2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
cyl1< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = leg_vectors< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Coordinate systems< / span > < / span >
stewart.lower_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
stewart.upper_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > i< / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
stewart.lower_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > '< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.upper_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ', cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > '< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Position Matrix< / span > < / span >
stewart.M_pos_base = stewart.pos_base < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > height< span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > stewart.TP.thickness< span class = "org-type" > +< / span > stewart.Leg.sphere.top< span class = "org-type" > +< / span > stewart.SP.thickness.top< span class = "org-type" > +< / span > stewart.jacobian< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute Jacobian Matrix< / span > < / span >
aa = stewart.pos_top_tranform < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.jacobian < span class = "org-type" > -< / span > stewart.TP.thickness < span class = "org-type" > -< / span > stewart.SP.height.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.J = getJacobianMatrix< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors', aa'< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > J< / span > = < span class = "org-function-name" > getJacobianMatrix< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > RM< / span > ,< span class = "org-variable-name" > M_pos_base< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
% RM< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] unit vector of each leg in the fixed frame
% M_pos_base< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] vector of the leg connection at the top platform location in the fixed frame
J = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = RM';
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > M_pos_base, RM< span class = "org-rainbow-delimiters-depth-1" > )< / span > ';
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org33550ef" class = "outline-3" >
< h3 id = "org33550ef" > < span class = "section-number-3" > 7.13< / span > Sample< / h3 >
< div class = "outline-text-3" id = "text-7-13" >
< p >
< a id = "org58a6fab" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "src/initializeSample.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > sample< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeSample< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
sample = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'radius'< / span > , < span class = "org-highlight-numbers-number" > 100< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'height'< / span > , < span class = "org-highlight-numbers-number" > 300< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'mass'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'offset'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , < span class = "org-underline" > ...< / span >
< span class = "org-string" > 'color'< / span > , < span class = "org-rainbow-delimiters-depth-2" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 45< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 45< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 45< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > < span class = "org-underline" > ...< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param','var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > opt< / span > = < span class = "org-constant" > fieldnames< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > opts_param< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-constant" > '< / span >
sample.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = opts_param.< span class = "org-rainbow-delimiters-depth-1" > (< / span > opt< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
sample.k.x = < span class = "org-highlight-numbers-number" > 1e8< / span > ;
sample.c.x = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > sample.k.x< span class = "org-type" > *< / span > sample.mass< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 10< / span > ;
sample.k.y = < span class = "org-highlight-numbers-number" > 1e8< / span > ;
sample.c.y = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > sample.k.y< span class = "org-type" > *< / span > sample.mass< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 10< / span > ;
sample.k.z = < span class = "org-highlight-numbers-number" > 1e8< / span > ;
sample.c.z = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > sample.k.y< span class = "org-type" > *< / span > sample.mass< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 10< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat', 'sample', '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< / div >
2019-07-12 11:53:59 +02:00
< / div >
< div id = "postamble" class = "status" >
< p class = "author" > Author: Dehaeze Thomas< / p >
2019-10-08 11:03:19 +02:00
< p class = "date" > Created: 2019-07-15 lun. 11:07< / p >
2019-07-12 11:53:59 +02:00
< p class = "validation" > < a href = "http://validator.w3.org/check?uri=referer" > Validate< / a > < / p >
< / div >
< / body >
< / html >
