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-29 10:52:59 +01:00
<!-- 2019 - 10 - 29 mar. 10:52 -->
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" / >
2019-10-08 11:19:45 +02:00
< title > Simscape Model of the Nano-Active-Stabilization-System< / title >
2019-07-12 11:53:59 +02:00
< meta name = "generator" content = "Org mode" / >
2019-10-08 11:19:45 +02:00
< meta name = "author" content = "Dehaeze Thomas" / >
2019-07-12 11:53:59 +02:00
< 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 >
2019-10-08 11:19:45 +02:00
< 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 >
2019-07-12 11:53:59 +02:00
< 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:19:45 +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" >
< h1 class = "title" > Simscape Model of the Nano-Active-Stabilization-System< / h1 >
< div id = "table-of-contents" >
< h2 > Table of Contents< / h2 >
< div id = "text-table-of-contents" >
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#org6af5373" > 1. Simulink project< / a > < / li >
< li > < a href = "#org2e621fc" > 2. Simscape Model< / a >
2019-10-08 11:19:45 +02:00
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orga525a01" > 2.1. Solid bodies< / a > < / li >
< li > < a href = "#orgfcbb75d" > 2.2. Frames< / a > < / li >
< li > < a href = "#orga9f9a27" > 2.3. Joints< / a > < / li >
< li > < a href = "#org09ee778" > 2.4. Measurements< / a > < / li >
< li > < a href = "#org318de2c" > 2.5. Excitation< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
< / li >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orgc7f7e1c" > 3. Notes< / a >
2019-10-08 11:19:45 +02:00
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orgb12490c" > 3.1. Simscape files for identification< / a > < / li >
< li > < a href = "#orgb836c01" > 3.2. Inputs< / a >
2019-10-08 11:19:45 +02:00
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orgf1bf5dd" > 3.2.1. Perturbations< / a > < / li >
< li > < a href = "#org0810896" > 3.2.2. Measurement Noise< / a > < / li >
< li > < a href = "#org5c2d540" > 3.2.3. Control Inputs< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
< / li >
2019-10-29 10:52:59 +01:00
< li > < a href = "#org6e0a79a" > 3.3. Outputs< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
< / li >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orgd49784d" > 4. Simulink files< / a > < / li >
< li > < a href = "#orgc98892e" > 5. Simulink Library< / a >
2019-10-08 11:19:45 +02:00
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orgb55d4ab" > 5.1. inputs< / a > < / li >
< li > < a href = "#org41c1e10" > 5.2. nass< sub > library< / sub > < / a > < / li >
< li > < a href = "#orgded2da1" > 5.3. pos< sub > error< / sub > < sub > wrt< / sub > < sub > nass< / sub > < sub > base< / sub > < / a > < / li >
< li > < a href = "#org7c4f21d" > 5.4. QuaternionToAngles< / a > < / li >
< li > < a href = "#org9c713f6" > 5.5. RotationMatrixToAngle< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
< / li >
2019-10-29 10:52:59 +01:00
< li > < a href = "#org3048f9a" > 6. Scripts< / a >
2019-10-08 11:19:45 +02:00
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orgff52b6c" > 6.1. Simulation Initialization< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
< / li >
2019-10-29 10:52:59 +01:00
< li > < a href = "#org28ec0bd" > 7. Functions< / a >
2019-10-08 11:19:45 +02:00
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#org0dfda5a" > 7.1. computePsdDispl< / a > < / li >
< li > < a href = "#org553eea9" > 7.2. computeSetpoint< / a > < / li >
< li > < a href = "#org786af26" > 7.3. converErrorBasis< / a > < / li >
< li > < a href = "#org9ea9883" > 7.4. generateDiagPidControl< / a > < / li >
< li > < a href = "#org219d806" > 7.5. identifyPlant< / a > < / li >
< li > < a href = "#orgffb2167" > 7.6. runSimulation< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
< / li >
2019-10-29 10:52:59 +01:00
< li > < a href = "#orgcee9dda" > 8. Initialize Elements< / a >
2019-10-08 11:19:45 +02:00
< ul >
2019-10-29 10:52:59 +01:00
< li > < a href = "#org870f9ac" > 8.1. Simulation Configuration< / a > < / li >
< li > < a href = "#orge30fdf8" > 8.2. Experiment< / a > < / li >
< li > < a href = "#orgab84833" > 8.3. Inputs< / a > < / li >
< li > < a href = "#org103cc41" > 8.4. Ground< / a > < / li >
< li > < a href = "#org6619477" > 8.5. Granite< / a > < / li >
< li > < a href = "#org77a28f7" > 8.6. Translation Stage< / a > < / li >
< li > < a href = "#orge78d8a3" > 8.7. Tilt Stage< / a > < / li >
< li > < a href = "#org567c23d" > 8.8. Spindle< / a > < / li >
< li > < a href = "#orgc032b1e" > 8.9. Micro Hexapod< / a > < / li >
< li > < a href = "#org3b5edf8" > 8.10. Center of gravity compensation< / a > < / li >
< li > < a href = "#orgf300722" > 8.11. Mirror< / a > < / li >
< li > < a href = "#orgf57d858" > 8.12. Nano Hexapod< / a > < / li >
< li > < a href = "#org2e21283" > 8.13. Sample< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
< / li >
< / ul >
< / div >
< / div >
< ul class = "org-ul" >
< li > < a href = "identification/index.html" > Identification of the Micro-Station< / a > < / li >
2019-10-24 17:45:30 +02:00
< li > < a href = "uniaxial/index.html" > Uniaxial Model< / a > < / li >
2019-10-08 11:19:45 +02:00
< li > < a href = "kinematics/index.html" > Kinematics of the station< / a > < / li >
< li > < a href = "control/index.html" > Control< / a > < / li >
< li > < a href = "active_damping/index.html" > Active Damping< / a > < / li >
< li > < a href = "analysis/index.html" > Plant Analysis< / a > < / li >
< li > < a href = "hac_lac/index.html" > HAC LAC< / a > < / li >
< li > < a href = "kinematics/index.html" > Kinematics< / a > < / li >
< li > < a href = "modal_test/index.html" > Modal Analysis< / a > < / li >
2019-10-29 10:52:59 +01:00
< li > < a href = "cedrat-actuator/index.html" > Cedrat Piezo Actuator< / a > < / li >
2019-10-08 11:19:45 +02:00
< / ul >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org6af5373" class = "outline-2" >
< h2 id = "org6af5373" > < span class = "section-number-2" > 1< / span > Simulink project< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-1" >
< p >
From the < a href = "https://mathworks.com/products/simulink/projects.html" > Simulink project< / a > mathworks page:
< / p >
< blockquote >
< p >
Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place.
< / p >
< p >
With Simulink Projects, you can:
< / p >
< ul class = "org-ul" >
< li > < b > Collaborate< / b > : Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.< / li >
< li > < b > Automate< / b > : Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.< / li >
< li > < b > Integrate with source control< / b > : Enable easy integration with source control and configuration management tools.< / li >
< / ul >
< / blockquote >
< 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" > 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 > ;
2019-10-24 17:45:30 +02:00
save_fig = < span class = "org-constant" > false< / span > ;
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/config.mat'< / span > , < span class = "org-string" > 'freqs'< / span > , < span class = "org-string" > 'save_fig'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
Simulink.fileGenControl< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'set'< / span > ,...
< span class = "org-string" > 'CacheFolder'< / span > , myCacheFolder,...
< span class = "org-string" > 'CodeGenFolder'< / span > , myCodeFolder,...
< span class = "org-string" > 'createDir'< / span > , < span class = "org-constant" > true< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org2e621fc" class = "outline-2" >
< h2 id = "org2e621fc" > < span class = "section-number-2" > 2< / span > Simscape Model< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-2" >
< p >
A < a href = "https://.mathworks.com/products/simscape.html" > simscape< / a > model permits to model multi-physics systems.
< / p >
< p >
< a href = "https://mathworks.com/products/simmechanics.html" > Simscape Multibody< / a > permits to model multibody systems using blocks representing bodies, joints, constraints, force elements, and sensors.
< / p >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orga525a01" class = "outline-3" >
< h3 id = "orga525a01" > < span class = "section-number-3" > 2.1< / span > Solid bodies< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-2-1" >
< p >
Each solid body is represented by a < a href = "https://mathworks.com/help/physmod/sm/ref/solid.html" > solid block< / a > .
The geometry of the solid body can be imported using a < code > step< / code > file. The properties of the solid body such as its mass, center of mass and moment of inertia can be derived from its density and its geometry as defined by the < code > step< / code > file. They also can be set by hand.
< / p >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgfcbb75d" class = "outline-3" >
< h3 id = "orgfcbb75d" > < span class = "section-number-3" > 2.2< / span > Frames< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-2-2" >
< p >
Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made.
< / p >
< p >
They can be defined from the solid body geometry, or using the < a href = "https://mathworks.com/help/physmod/sm/ref/rigidtransform.html" > rigid transform block< / a > .
< / p >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orga9f9a27" class = "outline-3" >
< h3 id = "orga9f9a27" > < span class = "section-number-3" > 2.3< / span > Joints< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-2-3" >
< p >
Solid Bodies are connected with joints (between frames of the two solid bodies).
< / p >
< p >
There are various types of joints that are all described < a href = "https://mathworks.com/help/physmod/sm/ug/joints.html" > here< / a > .
< / p >
2019-10-29 10:52:59 +01:00
< table id = "orgbcf1bb7" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-08 11:19:45 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 1:< / span > Degrees of freedom associated with each joint< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-right" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Joint Block< / th >
< th scope = "col" class = "org-right" > Translational DOFs< / th >
< th scope = "col" class = "org-right" > Rotational DOFs< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > 6-DOF< / td >
< td class = "org-right" > 3< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Bearing< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Bushing< / td >
< td class = "org-right" > 3< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Cartesian< / td >
< td class = "org-right" > 3< / td >
< td class = "org-right" > 0< / td >
< / tr >
< tr >
< td class = "org-left" > Constant Velocity< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 2< / td >
< / tr >
< tr >
< td class = "org-left" > Cylindrical< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Gimbal< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Leadscrew< / td >
< td class = "org-right" > 1 (coupled)< / td >
< td class = "org-right" > 1 (coupled)< / td >
< / tr >
< tr >
< td class = "org-left" > Pin Slot< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Planar< / td >
< td class = "org-right" > 2< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Prismatic< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 0< / td >
< / tr >
< tr >
< td class = "org-left" > Rectangular< / td >
< td class = "org-right" > 2< / td >
< td class = "org-right" > 0< / td >
< / tr >
< tr >
< td class = "org-left" > Revolute< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Spherical< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Telescoping< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Universal< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 2< / td >
< / tr >
< tr >
< td class = "org-left" > Weld< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 0< / td >
< / tr >
< / tbody >
< / table >
< p >
Joint blocks are assortments of joint primitives:
< / p >
< ul class = "org-ul" >
< li > < b > Prismatic< / b > : allows translation along a single standard axis: < code > Px< / code > , < code > Py< / code > , < code > Pz< / code > < / li >
< li > < b > Revolute< / b > : allows rotation about a single standard axis: < code > Rx< / code > , < code > Ry< / code > , < code > Rz< / code > < / li >
< li > < b > Spherical< / b > : allow rotation about any 3D axis: < code > S< / code > < / li >
< li > < b > Lead Screw< / b > : allows coupled rotation and translation on a standard axis: < code > LSz< / code > < / li >
< li > < b > Constant Velocity< / b > : Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: < code > CV< / code > < / li >
< / ul >
2019-10-29 10:52:59 +01:00
< table id = "orgcdffc88" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-08 11:19:45 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 2:< / span > Joint primitives for each joint type< / caption >
< 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" / >
< 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" > Joint Block< / th >
< th scope = "col" class = "org-left" > Px< / th >
< th scope = "col" class = "org-left" > Py< / th >
< th scope = "col" class = "org-left" > Pz< / th >
< th scope = "col" class = "org-left" > Rx< / th >
< th scope = "col" class = "org-left" > Ry< / th >
< th scope = "col" class = "org-left" > Rz< / th >
< th scope = "col" class = "org-left" > S< / th >
< th scope = "col" class = "org-left" > CV< / th >
< th scope = "col" class = "org-left" > LSz< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > 6-DOF< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Bearing< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Bushing< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Cartesian< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Constant Velocity< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Cylindrical< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Gimbal< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Leadscrew< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< / tr >
< tr >
< td class = "org-left" > Pin Slot< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Planar< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Prismatic< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Rectangular< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Revolute< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Spherical< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Telescoping< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Universal< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Weld< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< / tbody >
< / table >
< p >
For each of the joint primitive, we can specify the dynamical properties:
< / p >
< ul class = "org-ul" >
< li > The < b > spring stiffness< / b > : either linear or rotational one< / li >
< li > The < b > damping coefficient< / b > < / li >
< / ul >
< p >
For the actuation, we can either specify the motion or the force:
< / p >
< ul class = "org-ul" >
< li > the force applied in the corresponding DOF is provided by the input< / li >
< li > the motion is provided by the input< / li >
< / ul >
< p >
A sensor can be added to measure either the position, velocity or acceleration of the joint DOF.
< / p >
< p >
Composite Force/Torque sensing:
< / p >
< ul class = "org-ul" >
< li > Constraint force< / li >
< li > Total force: gives the sum across all joint primitives over all sources: actuation inputs, internal springs and dampers.< / li >
< / ul >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org09ee778" class = "outline-3" >
< h3 id = "org09ee778" > < span class = "section-number-3" > 2.4< / span > Measurements< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-2-4" >
< p >
A transform sensor block measures the spatial relationship between two frames: the base < code > B< / code > and the follower < code > F< / code > .
< / p >
< p >
It can give the rotational and translational position, velocity and acceleration.
< / p >
< p >
The measurement frame should be specified: it corresponds to the frame in which to resolve the selected spatial measurements. The default is < code > world< / code > .
< / p >
< p >
If we want to simulate an < b > inertial sensor< / b > , we just have to choose < code > B< / code > to be the < code > world< / code > frame.
< / p >
< p >
< b > Force sensors< / b > are included in the joints blocks.
< / p >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org318de2c" class = "outline-3" >
< h3 id = "org318de2c" > < span class = "section-number-3" > 2.5< / span > Excitation< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-2-5" >
< p >
We can apply < b > external forces< / b > to the model by using an < a href = "https://mathworks.com/help/physmod/sm/ref/externalforceandtorque.html" > external force and torque block< / a > .
< / p >
< p >
Internal force, acting reciprocally between base and following origins is implemented using the < a href = "https://mathworks.com/help/physmod/sm/ref/internalforce.html" > internal force block< / a > even though it is usually included in one joint block.
< / p >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgc7f7e1c" class = "outline-2" >
< h2 id = "orgc7f7e1c" > < span class = "section-number-2" > 3< / span > Notes< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-3" >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgb12490c" class = "outline-3" >
< h3 id = "orgb12490c" > < span class = "section-number-3" > 3.1< / span > Simscape files for identification< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-3-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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgb836c01" class = "outline-3" >
< h3 id = "orgb836c01" > < span class = "section-number-3" > 3.2< / span > Inputs< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-3-2" >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgf1bf5dd" class = "outline-4" >
< h4 id = "orgf1bf5dd" > < span class = "section-number-4" > 3.2.1< / span > Perturbations< / h4 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-4" id = "text-3-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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org0810896" class = "outline-4" >
< h4 id = "org0810896" > < span class = "section-number-4" > 3.2.2< / span > Measurement Noise< / h4 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-4" id = "text-3-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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org5c2d540" class = "outline-4" >
< h4 id = "org5c2d540" > < span class = "section-number-4" > 3.2.3< / span > Control Inputs< / h4 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-4" id = "text-3-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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org6e0a79a" class = "outline-3" >
< h3 id = "org6e0a79a" > < span class = "section-number-3" > 3.3< / span > Outputs< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-3-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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgd49784d" class = "outline-2" >
< h2 id = "orgd49784d" > < span class = "section-number-2" > 4< / span > Simulink files< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-4" >
< 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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgc98892e" class = "outline-2" >
< h2 id = "orgc98892e" > < span class = "section-number-2" > 5< / span > Simulink Library< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-5" >
< p >
A simulink library is developed in order to share elements between the different simulink files.
< / p >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgb55d4ab" class = "outline-3" >
< h3 id = "orgb55d4ab" > < span class = "section-number-3" > 5.1< / span > inputs< / h3 >
2019-10-08 11:19:45 +02:00
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org41c1e10" class = "outline-3" >
< h3 id = "org41c1e10" > < span class = "section-number-3" > 5.2< / span > nass< sub > library< / sub > < / h3 >
2019-10-08 11:19:45 +02:00
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgded2da1" class = "outline-3" >
< h3 id = "orgded2da1" > < span class = "section-number-3" > 5.3< / span > pos< sub > error< / sub > < sub > wrt< / sub > < sub > nass< / sub > < sub > base< / sub > < / h3 >
2019-10-08 11:19:45 +02:00
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org7c4f21d" class = "outline-3" >
< h3 id = "org7c4f21d" > < span class = "section-number-3" > 5.4< / span > QuaternionToAngles< / h3 >
2019-10-08 11:19:45 +02:00
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org9c713f6" class = "outline-3" >
< h3 id = "org9c713f6" > < span class = "section-number-3" > 5.5< / span > RotationMatrixToAngle< / h3 >
2019-10-08 11:19:45 +02:00
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org3048f9a" class = "outline-2" >
< h2 id = "org3048f9a" > < span class = "section-number-2" > 6< / span > Scripts< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-6" >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgff52b6c" class = "outline-3" >
< h3 id = "orgff52b6c" > < span class = "section-number-3" > 6.1< / span > Simulation Initialization< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-6-1" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org1cdf096" > < / a >
2019-10-08 11:19:45 +02:00
< / 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" > '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" > 'controllers.mat'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org28ec0bd" class = "outline-2" >
< h2 id = "org28ec0bd" > < span class = "section-number-2" > 7< / span > Functions< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-7" >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org0dfda5a" class = "outline-3" >
< h3 id = "org0dfda5a" > < span class = "section-number-3" > 7.1< / span > computePsdDispl< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-7-1" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orgb5eb383" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 > ;
2019-10-24 17:45:30 +02:00
psd_object = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'f'< / span > , f, ...
< span class = "org-string" > 'dx'< / span > , pdx, ...
< span class = "org-string" > 'dy'< / span > , pdy, ...
< span class = "org-string" > 'dz'< / span > , pdz, ...
< span class = "org-string" > 'rx'< / span > , prx, ...
< span class = "org-string" > 'ry'< / span > , pry, ...
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org553eea9" class = "outline-3" >
< h3 id = "org553eea9" > < span class = "section-number-3" > 7.2< / span > computeSetpoint< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-7-2" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orgd3c6bc1" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org786af26" class = "outline-3" >
< h3 id = "org786af26" > < span class = "section-number-3" > 7.3< / span > converErrorBasis< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-7-3" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orga13b9f1" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
pos = pos< span class = "org-type" > '< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
setpoint = setpoint< span class = "org-type" > '< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org9ea9883" class = "outline-3" >
< h3 id = "org9ea9883" > < span class = "section-number-3" > 7.4< / span > generateDiagPidControl< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-7-4" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orgdff51b6" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
pid_opts = pidtuneOptions< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'PhaseMargin'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > , ...
< span class = "org-string" > 'DesignFocus'< / span > , < span class = "org-string" > 'disturbance-rejection'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org219d806" class = "outline-3" >
< h3 id = "org219d806" > < span class = "section-number-3" > 7.5< / span > identifyPlant< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-7-5" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org0ed2644" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
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-string" > '/Fn'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < 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-string" > '/Dw'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < 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-string" > '/Fs'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < 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-string" > '/Fnl'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < 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-string" > '/Dsm'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < 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-string" > '/Fnlm'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < 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-string" > '/Dnlm'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < 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-string" > '/Es'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Position Error w.r.t. NASS base< / span >
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
G.InputName = < span class = "org-rainbow-delimiters-depth-1" > {< / span > < span class = "org-string" > 'Fnx'< / span > , < span class = "org-string" > 'Fny'< / span > , < span class = "org-string" > 'Fnz'< / span > , < span class = "org-string" > 'Mnx'< / span > , < span class = "org-string" > 'Mny'< / span > , < span class = "org-string" > 'Mnz'< / span > , ...
< span class = "org-string" > 'Dgx'< / span > , < span class = "org-string" > 'Dgy'< / span > , < span class = "org-string" > 'Dgz'< / span > , ...
< span class = "org-string" > 'Fsx'< / span > , < span class = "org-string" > 'Fsy'< / span > , < span class = "org-string" > 'Fsz'< / span > , < span class = "org-string" > 'Msx'< / span > , < span class = "org-string" > 'Msy'< / span > , < span class = "org-string" > 'Msz'< / span > , ...
< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > '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'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > , ...
< span class = "org-string" > 'Fm1'< / span > , < span class = "org-string" > 'Fm2'< / span > , < span class = "org-string" > 'Fm3'< / span > , < span class = "org-string" > 'Fm4'< / span > , < span class = "org-string" > 'Fm5'< / span > , < span class = "org-string" > 'Fm6'< / span > , ...
< span class = "org-string" > 'Dm1'< / span > , < span class = "org-string" > 'Dm2'< / span > , < span class = "org-string" > 'Dm3'< / span > , < span class = "org-string" > 'Dm4'< / span > , < span class = "org-string" > 'Dm5'< / span > , < span class = "org-string" > 'Dm6'< / span > , ...
< span class = "org-string" > 'Edx'< / span > , < span class = "org-string" > 'Rdy'< / span > , < span class = "org-string" > 'Edz'< / span > , < span class = "org-string" > 'Erx'< / span > , < span class = "org-string" > 'Ery'< / span > , < span class = "org-string" > 'Erz'< / span > < span class = "org-rainbow-delimiters-depth-1" > }< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
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'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fnx'< / span > , < span class = "org-string" > 'Fny'< / span > , < span class = "org-string" > 'Fnz'< / span > , < span class = "org-string" > 'Mnx'< / span > , < span class = "org-string" > 'Mny'< / span > , < span class = "org-string" > '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 > ;
2019-10-08 11:19:45 +02:00
< span class = "org-comment" > % From ground motion to Sample displacement< / span >
2019-10-24 17:45:30 +02:00
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'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dgx'< / span > , < span class = "org-string" > 'Dgy'< / span > , < span class = "org-string" > '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 > ;
2019-10-08 11:19:45 +02:00
< span class = "org-comment" > % From direct forces applied on the sample to displacement of the sample< / span >
2019-10-24 17:45:30 +02:00
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'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fsx'< / span > , < span class = "org-string" > 'Fsy'< / span > , < span class = "org-string" > 'Fsz'< / span > , < span class = "org-string" > 'Msx'< / span > , < span class = "org-string" > 'Msy'< / span > , < span class = "org-string" > '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 > ;
2019-10-08 11:19:45 +02:00
< span class = "org-comment" > % From forces applied on NASS's legs to force sensor in each leg< / span >
2019-10-24 17:45:30 +02:00
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'< / span > , < span class = "org-string" > 'Fm2'< / span > , < span class = "org-string" > 'Fm3'< / span > , < span class = "org-string" > 'Fm4'< / span > , < span class = "org-string" > 'Fm5'< / span > , < span class = "org-string" > 'Fm6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > '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 > ;
2019-10-08 11:19:45 +02:00
< span class = "org-comment" > % From forces applied on NASS's legs to displacement of each leg< / span >
2019-10-24 17:45:30 +02:00
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'< / span > , < span class = "org-string" > 'Dm2'< / span > , < span class = "org-string" > 'Dm3'< / span > , < span class = "org-string" > 'Dm4'< / span > , < span class = "org-string" > 'Dm5'< / span > , < span class = "org-string" > 'Dm6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > '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 > ;
2019-10-08 11:19:45 +02:00
< span class = "org-comment" > % From forces applied on NASS's legs to displacement of each leg< / span >
2019-10-24 17:45:30 +02:00
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'< / span > , < span class = "org-string" > 'Rdy'< / span > , < span class = "org-string" > 'Edz'< / span > , < span class = "org-string" > 'Erx'< / span > , < span class = "org-string" > 'Ery'< / span > , < span class = "org-string" > 'Erz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fnx'< / span > , < span class = "org-string" > 'Fny'< / span > , < span class = "org-string" > 'Fnz'< / span > , < span class = "org-string" > 'Mnx'< / span > , < span class = "org-string" > 'Mny'< / span > , < span class = "org-string" > '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 > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgffb2167" class = "outline-3" >
< h3 id = "orgffb2167" > < span class = "section-number-3" > 7.6< / span > runSimulation< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-7-6" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orge0cdc60" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
< 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-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > act_damp, < span class = "org-string" > 'none'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > '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-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > & & < / span > strcmp< span class = "org-rainbow-delimiters-depth-3" > (< / span > act_damp, < span class = "org-string" > 'iff'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K_iff'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K_iff'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
initializeNanoHexapod< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'actuator'< / span > , < span class = "org-string" > 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
initializeNanoHexapod< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'actuator'< / span > , < span class = "org-string" > 'lorentz'< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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);
2019-10-24 17:45:30 +02:00
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-string" > 'time'< / span > , < span class = "org-string" > 'Dx'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > , < span class = "org-string" > 'K'< / span > < span class = "org-rainbow-delimiters-depth-9" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgcee9dda" class = "outline-2" >
< h2 id = "orgcee9dda" > < span class = "section-number-2" > 8< / span > Initialize Elements< / h2 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-2" id = "text-8" >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org870f9ac" class = "outline-3" >
< h3 id = "org870f9ac" > < span class = "section-number-3" > 8.1< / span > Simulation Configuration< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-1" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org0e8d931" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
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-comment" > % Sampling time [s]< / span >
< span class = "org-string" > 'Tsim'< / span > , < span class = "org-highlight-numbers-number" > 10< / 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-comment" > % Close Loop time [s]< / span >
< span class = "org-string" > 'gravity'< / span > , < span class = "org-constant" > false< / span > ...< span class = "org-comment" > % Gravity along the z axis< / span >
2019-10-08 11:19:45 +02:00
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/sim_conf.mat'< / span > , < span class = "org-string" > 'sim_conf'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orge30fdf8" class = "outline-3" >
< h3 id = "orge30fdf8" > < span class = "section-number-3" > 8.2< / span > Experiment< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-2" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org27259e7" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
opts_sim = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Tsim'< / span > , < span class = "org-highlight-numbers-number" > 5< / span > , ...
< span class = "org-string" > 'cl_time'< / span > , < span class = "org-highlight-numbers-number" > 5< / span > ...
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
opts_inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Dw'< / span > , < span class = "org-constant" > true< / span > , ...
< span class = "org-string" > 'Rz'< / span > , < span class = "org-highlight-numbers-number" > 60< / span > ...< span class = "org-comment" > % rpm< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
opts_inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Dw'< / span > , < span class = "org-constant" > true< / span > , ...
< span class = "org-string" > 'Rz'< / span > , < span class = "org-highlight-numbers-number" > 1< / span > ...< span class = "org-comment" > % rpm< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgab84833" class = "outline-3" >
< h3 id = "orgab84833" > < span class = "section-number-3" > 8.3< / span > Inputs< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-3" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org9834d3d" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Dw'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Dy'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Ry'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Rz'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Dh'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Rm'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Dn'< / span > , < span class = "org-constant" > false< / span > ...
2019-10-08 11:19:45 +02:00
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/sim_conf.mat'< / span > , < span class = "org-string" > 'sim_conf'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > true< / span >
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/perturbations.mat'< / span > , < span class = "org-string" > 'Wxg'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > false< / span >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > true< / span >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > false< / span >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > true< / span >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > false< / span >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > true< / span >
2019-10-08 11:19:45 +02:00
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;
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > false< / span >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > true< / span >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > false< / span >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > true< / span >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< 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 > < span class = "org-constant" > false< / span >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Ts'< / span > , sim_conf.Ts, ...
< 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-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-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-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-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-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-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-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-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-string" > 'Fs'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Fs, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > ...
2019-10-08 11:19:45 +02:00
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/inputs.mat'< / span > , < span class = "org-string" > 'inputs'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org103cc41" class = "outline-3" >
< h3 id = "org103cc41" > < span class = "section-number-3" > 8.4< / span > Ground< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-4" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orga991e31" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'ground'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org6619477" class = "outline-3" >
< h3 id = "org6619477" > < span class = "section-number-3" > 8.5< / span > Granite< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-5" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org15c259d" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'granite'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org77a28f7" class = "outline-3" >
< h3 id = "org77a28f7" > < span class = "section-number-3" > 8.6< / span > Translation Stage< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-6" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org98bfb5b" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'ty'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orge78d8a3" class = "outline-3" >
< h3 id = "orge78d8a3" > < span class = "section-number-3" > 8.7< / span > Tilt Stage< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-7" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org87f3ecd" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'ry'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org567c23d" class = "outline-3" >
< h3 id = "org567c23d" > < span class = "section-number-3" > 8.8< / span > Spindle< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-8" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orga6d2fea" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'rz'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgc032b1e" class = "outline-3" >
< h3 id = "orgc032b1e" > < span class = "section-number-3" > 8.9< / span > Micro Hexapod< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-9" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orgbd011a3" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
% micro_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 269< / span > .< span class = "org-highlight-numbers-number" > 26< / span > ; % Distance from the top platform to the Jacobian point [mm]
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'micro_hexapod'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
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 > ; ...
stewart.Leg.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; ...
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
stewart.Leg.rad.top stewart.Leg.lenght; ...
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; ...
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 > < span class = "org-type" > '< / 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-type" > '< / 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-type" > '< / 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 > < span class = "org-type" > '< / 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-type" > '< / 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-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
stewart.J = getJacobianMatrix< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-type" > '< / span > , aa< span class = "org-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
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< span class = "org-type" > '< / span > ;
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-type" > '< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org3b5edf8" class = "outline-3" >
< h3 id = "org3b5edf8" > < span class = "section-number-3" > 8.10< / span > Center of gravity compensation< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-10" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org908ebcf" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'axisc'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgf300722" class = "outline-3" >
< h3 id = "orgf300722" > < span class = "section-number-3" > 8.11< / span > Mirror< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-11" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "orgb5894e5" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'shape'< / span > , < span class = "org-string" > 'spherical'< / span > , ...< span class = "org-comment" > % spherical or conical< / span >
< span class = "org-string" > 'angle'< / span > , < span class = "org-highlight-numbers-number" > 45< / span > ...
2019-10-08 11:19:45 +02:00
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > ...
2019-10-08 11:19:45 +02:00
< span class = "org-highlight-numbers-number" > 0< / span > mirror.h< span class = "org-type" > -< / span > mirror.thickness
2019-10-24 17:45:30 +02:00
mirror.hole_rad mirror.h< span class = "org-type" > -< / span > mirror.thickness; ...
mirror.hole_rad < span class = "org-highlight-numbers-number" > 0< / span > ; ...
mirror.rad < span class = "org-highlight-numbers-number" > 0< / span > ...
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'mirror'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-orgf57d858" class = "outline-3" >
< h3 id = "orgf57d858" > < span class = "section-number-3" > 8.12< / span > Nano Hexapod< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-12" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org9b0c46a" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'actuator'< / span > , < span class = "org-string" > 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
% nano_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 174< / span > .< span class = "org-highlight-numbers-number" > 26< / span > ; % Point where the Jacobian is computed =< span class = "org-type" > > < / span > Center of rotation [mm]
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'nano_hexapod'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
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 > ; ...
stewart.Leg.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; ...
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
stewart.Leg.rad.top stewart.Leg.lenght; ...
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; ...
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 >
2019-10-08 11:19:45 +02:00
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 > ;
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / 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 > < span class = "org-type" > '< / 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-type" > '< / 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-type" > '< / 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 > < span class = "org-type" > '< / 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-type" > '< / 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-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
stewart.J = getJacobianMatrix< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-type" > '< / span > , aa< span class = "org-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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 > ;
2019-10-24 17:45:30 +02:00
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< span class = "org-type" > '< / span > ;
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-type" > '< / span > ;
2019-10-08 11:19:45 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-10-29 10:52:59 +01:00
< div id = "outline-container-org2e21283" class = "outline-3" >
< h3 id = "org2e21283" > < span class = "section-number-3" > 8.13< / span > Sample< / h3 >
2019-10-08 11:19:45 +02:00
< div class = "outline-text-3" id = "text-8-13" >
< p >
2019-10-29 10:52:59 +01:00
< a id = "org0a4b50f" > < / a >
2019-10-08 11:19:45 +02:00
< / 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 >
2019-10-24 17:45:30 +02:00
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-string" > 'height'< / span > , < span class = "org-highlight-numbers-number" > 300< / span > , ...
< span class = "org-string" > 'mass'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > , ...
< span class = "org-string" > 'offset'< / span > , < span class = "org-highlight-numbers-number" > 0< / 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 > ...
2019-10-08 11:19:45 +02:00
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
2019-10-24 17:45:30 +02:00
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-08 11:19:45 +02:00
< 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 >
2019-10-24 17:45:30 +02:00
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'sample'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-08 11:19:45 +02:00
< 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" >
2019-10-08 11:19:45 +02:00
< p class = "author" > Author: Dehaeze Thomas< / p >
2019-10-29 10:52:59 +01:00
< p class = "date" > Created: 2019-10-29 mar. 10:52< / 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 >
