3287 lines
347 KiB
HTML
3287 lines
347 KiB
HTML
<?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-12-10 mar. 18:05 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
|
<title>Simscape Model</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<style type="text/css">
|
|
<!--/*--><![CDATA[/*><!--*/
|
|
.title { text-align: center;
|
|
margin-bottom: .2em; }
|
|
.subtitle { text-align: center;
|
|
font-size: medium;
|
|
font-weight: bold;
|
|
margin-top:0; }
|
|
.todo { font-family: monospace; color: red; }
|
|
.done { font-family: monospace; color: green; }
|
|
.priority { font-family: monospace; color: orange; }
|
|
.tag { background-color: #eee; font-family: monospace;
|
|
padding: 2px; font-size: 80%; font-weight: normal; }
|
|
.timestamp { color: #bebebe; }
|
|
.timestamp-kwd { color: #5f9ea0; }
|
|
.org-right { margin-left: auto; margin-right: 0px; text-align: right; }
|
|
.org-left { margin-left: 0px; margin-right: auto; text-align: left; }
|
|
.org-center { margin-left: auto; margin-right: auto; text-align: center; }
|
|
.underline { text-decoration: underline; }
|
|
#postamble p, #preamble p { font-size: 90%; margin: .2em; }
|
|
p.verse { margin-left: 3%; }
|
|
pre {
|
|
border: 1px solid #ccc;
|
|
box-shadow: 3px 3px 3px #eee;
|
|
padding: 8pt;
|
|
font-family: monospace;
|
|
overflow: auto;
|
|
margin: 1.2em;
|
|
}
|
|
pre.src {
|
|
position: relative;
|
|
overflow: visible;
|
|
padding-top: 1.2em;
|
|
}
|
|
pre.src:before {
|
|
display: none;
|
|
position: absolute;
|
|
background-color: white;
|
|
top: -10px;
|
|
right: 10px;
|
|
padding: 3px;
|
|
border: 1px solid black;
|
|
}
|
|
pre.src:hover:before { display: inline;}
|
|
/* Languages per Org manual */
|
|
pre.src-asymptote:before { content: 'Asymptote'; }
|
|
pre.src-awk:before { content: 'Awk'; }
|
|
pre.src-C:before { content: 'C'; }
|
|
/* pre.src-C++ doesn't work in CSS */
|
|
pre.src-clojure:before { content: 'Clojure'; }
|
|
pre.src-css:before { content: 'CSS'; }
|
|
pre.src-D:before { content: 'D'; }
|
|
pre.src-ditaa:before { content: 'ditaa'; }
|
|
pre.src-dot:before { content: 'Graphviz'; }
|
|
pre.src-calc:before { content: 'Emacs Calc'; }
|
|
pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
|
|
pre.src-fortran:before { content: 'Fortran'; }
|
|
pre.src-gnuplot:before { content: 'gnuplot'; }
|
|
pre.src-haskell:before { content: 'Haskell'; }
|
|
pre.src-hledger:before { content: 'hledger'; }
|
|
pre.src-java:before { content: 'Java'; }
|
|
pre.src-js:before { content: 'Javascript'; }
|
|
pre.src-latex:before { content: 'LaTeX'; }
|
|
pre.src-ledger:before { content: 'Ledger'; }
|
|
pre.src-lisp:before { content: 'Lisp'; }
|
|
pre.src-lilypond:before { content: 'Lilypond'; }
|
|
pre.src-lua:before { content: 'Lua'; }
|
|
pre.src-matlab:before { content: 'MATLAB'; }
|
|
pre.src-mscgen:before { content: 'Mscgen'; }
|
|
pre.src-ocaml:before { content: 'Objective Caml'; }
|
|
pre.src-octave:before { content: 'Octave'; }
|
|
pre.src-org:before { content: 'Org mode'; }
|
|
pre.src-oz:before { content: 'OZ'; }
|
|
pre.src-plantuml:before { content: 'Plantuml'; }
|
|
pre.src-processing:before { content: 'Processing.js'; }
|
|
pre.src-python:before { content: 'Python'; }
|
|
pre.src-R:before { content: 'R'; }
|
|
pre.src-ruby:before { content: 'Ruby'; }
|
|
pre.src-sass:before { content: 'Sass'; }
|
|
pre.src-scheme:before { content: 'Scheme'; }
|
|
pre.src-screen:before { content: 'Gnu Screen'; }
|
|
pre.src-sed:before { content: 'Sed'; }
|
|
pre.src-sh:before { content: 'shell'; }
|
|
pre.src-sql:before { content: 'SQL'; }
|
|
pre.src-sqlite:before { content: 'SQLite'; }
|
|
/* additional languages in org.el's org-babel-load-languages alist */
|
|
pre.src-forth:before { content: 'Forth'; }
|
|
pre.src-io:before { content: 'IO'; }
|
|
pre.src-J:before { content: 'J'; }
|
|
pre.src-makefile:before { content: 'Makefile'; }
|
|
pre.src-maxima:before { content: 'Maxima'; }
|
|
pre.src-perl:before { content: 'Perl'; }
|
|
pre.src-picolisp:before { content: 'Pico Lisp'; }
|
|
pre.src-scala:before { content: 'Scala'; }
|
|
pre.src-shell:before { content: 'Shell Script'; }
|
|
pre.src-ebnf2ps:before { content: 'ebfn2ps'; }
|
|
/* additional language identifiers per "defun org-babel-execute"
|
|
in ob-*.el */
|
|
pre.src-cpp:before { content: 'C++'; }
|
|
pre.src-abc:before { content: 'ABC'; }
|
|
pre.src-coq:before { content: 'Coq'; }
|
|
pre.src-groovy:before { content: 'Groovy'; }
|
|
/* additional language identifiers from org-babel-shell-names in
|
|
ob-shell.el: ob-shell is the only babel language using a lambda to put
|
|
the execution function name together. */
|
|
pre.src-bash:before { content: 'bash'; }
|
|
pre.src-csh:before { content: 'csh'; }
|
|
pre.src-ash:before { content: 'ash'; }
|
|
pre.src-dash:before { content: 'dash'; }
|
|
pre.src-ksh:before { content: 'ksh'; }
|
|
pre.src-mksh:before { content: 'mksh'; }
|
|
pre.src-posh:before { content: 'posh'; }
|
|
/* Additional Emacs modes also supported by the LaTeX listings package */
|
|
pre.src-ada:before { content: 'Ada'; }
|
|
pre.src-asm:before { content: 'Assembler'; }
|
|
pre.src-caml:before { content: 'Caml'; }
|
|
pre.src-delphi:before { content: 'Delphi'; }
|
|
pre.src-html:before { content: 'HTML'; }
|
|
pre.src-idl:before { content: 'IDL'; }
|
|
pre.src-mercury:before { content: 'Mercury'; }
|
|
pre.src-metapost:before { content: 'MetaPost'; }
|
|
pre.src-modula-2:before { content: 'Modula-2'; }
|
|
pre.src-pascal:before { content: 'Pascal'; }
|
|
pre.src-ps:before { content: 'PostScript'; }
|
|
pre.src-prolog:before { content: 'Prolog'; }
|
|
pre.src-simula:before { content: 'Simula'; }
|
|
pre.src-tcl:before { content: 'tcl'; }
|
|
pre.src-tex:before { content: 'TeX'; }
|
|
pre.src-plain-tex:before { content: 'Plain TeX'; }
|
|
pre.src-verilog:before { content: 'Verilog'; }
|
|
pre.src-vhdl:before { content: 'VHDL'; }
|
|
pre.src-xml:before { content: 'XML'; }
|
|
pre.src-nxml:before { content: 'XML'; }
|
|
/* add a generic configuration mode; LaTeX export needs an additional
|
|
(add-to-list 'org-latex-listings-langs '(conf " ")) in .emacs */
|
|
pre.src-conf:before { content: 'Configuration File'; }
|
|
|
|
table { border-collapse:collapse; }
|
|
caption.t-above { caption-side: top; }
|
|
caption.t-bottom { caption-side: bottom; }
|
|
td, th { vertical-align:top; }
|
|
th.org-right { text-align: center; }
|
|
th.org-left { text-align: center; }
|
|
th.org-center { text-align: center; }
|
|
td.org-right { text-align: right; }
|
|
td.org-left { text-align: left; }
|
|
td.org-center { text-align: center; }
|
|
dt { font-weight: bold; }
|
|
.footpara { display: inline; }
|
|
.footdef { margin-bottom: 1em; }
|
|
.figure { padding: 1em; }
|
|
.figure p { text-align: center; }
|
|
.equation-container {
|
|
display: table;
|
|
text-align: center;
|
|
width: 100%;
|
|
}
|
|
.equation {
|
|
vertical-align: middle;
|
|
}
|
|
.equation-label {
|
|
display: table-cell;
|
|
text-align: right;
|
|
vertical-align: middle;
|
|
}
|
|
.inlinetask {
|
|
padding: 10px;
|
|
border: 2px solid gray;
|
|
margin: 10px;
|
|
background: #ffffcc;
|
|
}
|
|
#org-div-home-and-up
|
|
{ text-align: right; font-size: 70%; white-space: nowrap; }
|
|
textarea { overflow-x: auto; }
|
|
.linenr { font-size: smaller }
|
|
.code-highlighted { background-color: #ffff00; }
|
|
.org-info-js_info-navigation { border-style: none; }
|
|
#org-info-js_console-label
|
|
{ font-size: 10px; font-weight: bold; white-space: nowrap; }
|
|
.org-info-js_search-highlight
|
|
{ background-color: #ffff00; color: #000000; font-weight: bold; }
|
|
.org-svg { width: 90%; }
|
|
/*]]>*/-->
|
|
</style>
|
|
<link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
|
<link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
|
<link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
|
<script type="text/javascript" src="../js/jquery.min.js"></script>
|
|
<script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
|
<script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
|
<script type="text/javascript" src="../js/readtheorg.js"></script>
|
|
<script type="text/javascript">
|
|
/*
|
|
@licstart The following is the entire license notice for the
|
|
JavaScript code in this tag.
|
|
|
|
Copyright (C) 2012-2019 Free Software Foundation, Inc.
|
|
|
|
The JavaScript code in this tag is free software: you can
|
|
redistribute it and/or modify it under the terms of the GNU
|
|
General Public License (GNU GPL) as published by the Free Software
|
|
Foundation, either version 3 of the License, or (at your option)
|
|
any later version. The code is distributed WITHOUT ANY WARRANTY;
|
|
without even the implied warranty of MERCHANTABILITY or FITNESS
|
|
FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
|
|
|
|
As additional permission under GNU GPL version 3 section 7, you
|
|
may distribute non-source (e.g., minimized or compacted) forms of
|
|
that code without the copy of the GNU GPL normally required by
|
|
section 4, provided you include this license notice and a URL
|
|
through which recipients can access the Corresponding Source.
|
|
|
|
|
|
@licend The above is the entire license notice
|
|
for the JavaScript code in this tag.
|
|
*/
|
|
<!--/*--><![CDATA[/*><!--*/
|
|
function CodeHighlightOn(elem, id)
|
|
{
|
|
var target = document.getElementById(id);
|
|
if(null != target) {
|
|
elem.cacheClassElem = elem.className;
|
|
elem.cacheClassTarget = target.className;
|
|
target.className = "code-highlighted";
|
|
elem.className = "code-highlighted";
|
|
}
|
|
}
|
|
function CodeHighlightOff(elem, id)
|
|
{
|
|
var target = document.getElementById(id);
|
|
if(elem.cacheClassElem)
|
|
elem.className = elem.cacheClassElem;
|
|
if(elem.cacheClassTarget)
|
|
target.className = elem.cacheClassTarget;
|
|
}
|
|
/*]]>*///-->
|
|
</script>
|
|
</head>
|
|
<body>
|
|
<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</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#org88d59ba">1. Simulink Project - Startup and Shutdown scripts</a></li>
|
|
<li><a href="#orga2206d5">2. Simscape Multibody - Presentation</a>
|
|
<ul>
|
|
<li><a href="#org6b4540f">2.1. Solid bodies</a></li>
|
|
<li><a href="#orge470a34">2.2. Frames</a></li>
|
|
<li><a href="#org474edd3">2.3. Joints</a></li>
|
|
<li><a href="#org9171b40">2.4. Measurements</a></li>
|
|
<li><a href="#org7df2aef">2.5. Excitation</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org803cadc">3. Simulink files and signal names</a>
|
|
<ul>
|
|
<li><a href="#orgb5c767a">3.1. List of Simscape files</a></li>
|
|
<li><a href="#org64dbd0d">3.2. List of Inputs</a>
|
|
<ul>
|
|
<li><a href="#orgeecef4d">3.2.1. Perturbations</a></li>
|
|
<li><a href="#org4e58ed6">3.2.2. Measurement Noise</a></li>
|
|
<li><a href="#orgc9006ba">3.2.3. Control Inputs</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgeb985b0">3.3. List of Outputs</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgfd5289f">4. Simulink Library</a>
|
|
<ul>
|
|
<li><a href="#orgd1ebe0f">4.1. <code>inputs</code></a></li>
|
|
<li><a href="#org1ae1d37">4.2. <code>nass_library</code></a></li>
|
|
<li><a href="#org54e3d07">4.3. <code>pos_error_wrt_nass_base</code></a></li>
|
|
<li><a href="#orgc0f0f02">4.4. <code>QuaternionToAngles</code></a></li>
|
|
<li><a href="#orgbda19ed">4.5. <code>RotationMatrixToAngle</code></a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org54193c9">5. Functions</a>
|
|
<ul>
|
|
<li><a href="#orgc1f5840">5.1. computePsdDispl</a></li>
|
|
<li><a href="#org0d8e9e6">5.2. computeSetpoint</a></li>
|
|
<li><a href="#org9a191e3">5.3. converErrorBasis</a></li>
|
|
<li><a href="#org2d44efa">5.4. generateDiagPidControl</a></li>
|
|
<li><a href="#org6f7736a">5.5. identifyPlant</a></li>
|
|
<li><a href="#org32dd782">5.6. runSimulation</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org5cc2276">6. Initialize Elements</a>
|
|
<ul>
|
|
<li><a href="#org664a7e3">6.1. Experiment</a></li>
|
|
<li><a href="#org89c58f8">6.2. Generate Reference Signals</a></li>
|
|
<li><a href="#org77d160f">6.3. <span class="todo TODO">TODO</span> Inputs</a></li>
|
|
<li><a href="#org962187e">6.4. Ground</a></li>
|
|
<li><a href="#org198c366">6.5. Granite</a></li>
|
|
<li><a href="#org6e967d2">6.6. Translation Stage</a></li>
|
|
<li><a href="#org6c95b53">6.7. Tilt Stage</a></li>
|
|
<li><a href="#org13271e7">6.8. Spindle</a></li>
|
|
<li><a href="#org3c482fe">6.9. Initialize Hexapod legs' length</a></li>
|
|
<li><a href="#org7c295aa">6.10. Micro Hexapod</a></li>
|
|
<li><a href="#org72492f3">6.11. Center of gravity compensation</a></li>
|
|
<li><a href="#org68d031b">6.12. Mirror</a></li>
|
|
<li><a href="#org74149ff">6.13. Nano Hexapod</a></li>
|
|
<li><a href="#org94d9a68">6.14. Cedrat Actuator</a></li>
|
|
<li><a href="#org8ceaacc">6.15. Sample</a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
This file is used to explain how this Simscape Model works.
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>In section <a href="#orgfd7237b">1</a>, the simulink project with the associated scripts are presented</li>
|
|
<li>In section <a href="#orgb6586ca">2</a>, an introduction to Simscape Multibody is done</li>
|
|
<li>In section <a href="#org3e8132a">3</a>, each simscape files are presented with the associated signal names and joint architectures</li>
|
|
<li>In section <a href="#orgecb3837">4</a>, the list of the Simulink library elements are described</li>
|
|
<li>In section <a href="#org94ca66a">5</a>, a list of Matlab function that will be used are defined here</li>
|
|
<li>In section <a href="#org18b2c56">6</a>, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.</li>
|
|
</ul>
|
|
|
|
<div id="outline-container-org88d59ba" class="outline-2">
|
|
<h2 id="org88d59ba"><span class="section-number-2">1</span> Simulink Project - Startup and Shutdown scripts</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="orgfd7237b"></a>
|
|
</p>
|
|
|
|
<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">project = simulinkproject;
|
|
projectRoot = project.RootFolder;
|
|
|
|
myCacheFolder = fullfile<span class="org-rainbow-delimiters-depth-1">(</span>projectRoot, <span class="org-string">'.SimulinkCache'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
myCodeFolder = fullfile<span class="org-rainbow-delimiters-depth-1">(</span>projectRoot, <span class="org-string">'.SimulinkCode'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
Simulink.fileGenControl<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'set'</span>,...
|
|
<span class="org-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>;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
When the project closes, it runs the <code>project_shutdown.m</code> script defined below.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Simulink.fileGenControl<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'reset'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The project also permits to automatically add defined folder to the path when the project is opened.
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga2206d5" class="outline-2">
|
|
<h2 id="orga2206d5"><span class="section-number-2">2</span> Simscape Multibody - Presentation</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="orgb6586ca"></a>
|
|
</p>
|
|
|
|
<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>
|
|
|
|
<div id="outline-container-org6b4540f" class="outline-3">
|
|
<h3 id="org6b4540f"><span class="section-number-3">2.1</span> Solid bodies</h3>
|
|
<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>
|
|
|
|
<div id="outline-container-orge470a34" class="outline-3">
|
|
<h3 id="orge470a34"><span class="section-number-3">2.2</span> Frames</h3>
|
|
<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>
|
|
|
|
<div id="outline-container-org474edd3" class="outline-3">
|
|
<h3 id="org474edd3"><span class="section-number-3">2.3</span> Joints</h3>
|
|
<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>
|
|
|
|
<table id="org57b7cc8" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<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>
|
|
|
|
<table id="org75d3b05" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<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>
|
|
|
|
<div id="outline-container-org9171b40" class="outline-3">
|
|
<h3 id="org9171b40"><span class="section-number-3">2.4</span> Measurements</h3>
|
|
<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>
|
|
|
|
<div id="outline-container-org7df2aef" class="outline-3">
|
|
<h3 id="org7df2aef"><span class="section-number-3">2.5</span> Excitation</h3>
|
|
<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>
|
|
|
|
<div id="outline-container-org803cadc" class="outline-2">
|
|
<h2 id="org803cadc"><span class="section-number-2">3</span> Simulink files and signal names</h2>
|
|
<div class="outline-text-2" id="text-3">
|
|
<p>
|
|
<a id="org3e8132a"></a>
|
|
</p>
|
|
|
|
<p>
|
|
In order to "normalize" things, the names of all the signal are listed here.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb5c767a" class="outline-3">
|
|
<h3 id="orgb5c767a"><span class="section-number-3">3.1</span> List of Simscape files</h3>
|
|
<div class="outline-text-3" id="text-3-1">
|
|
<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>
|
|
|
|
<table id="org6eeaf61" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 3:</span> List of simscape files</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" />
|
|
</colgroup>
|
|
<thead>
|
|
<tr>
|
|
<th scope="col" class="org-left">Simscape Name</th>
|
|
<th scope="col" class="org-left">Ty</th>
|
|
<th scope="col" class="org-left">Ry</th>
|
|
<th scope="col" class="org-left">Rz</th>
|
|
<th scope="col" class="org-left">Hexa</th>
|
|
<th scope="col" class="org-left">NASS</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left">id micro station</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left"> </td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">id nano station stages</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left">F</td>
|
|
<td class="org-left">F</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">id nano station config</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">F</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">control nano station</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">D</td>
|
|
<td class="org-left">F</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org64dbd0d" class="outline-3">
|
|
<h3 id="org64dbd0d"><span class="section-number-3">3.2</span> List of Inputs</h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
</div>
|
|
<div id="outline-container-orgeecef4d" class="outline-4">
|
|
<h4 id="orgeecef4d"><span class="section-number-4">3.2.1</span> Perturbations</h4>
|
|
<div class="outline-text-4" id="text-3-2-1">
|
|
<table id="org5c60a9c" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 4:</span> List of Disturbances</caption>
|
|
|
|
<colgroup>
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
|
|
<col class="org-right" />
|
|
|
|
<col class="org-left" />
|
|
</colgroup>
|
|
<thead>
|
|
<tr>
|
|
<th scope="col" class="org-left">Variable</th>
|
|
<th scope="col" class="org-left">Meaning</th>
|
|
<th scope="col" class="org-right">Size</th>
|
|
<th scope="col" class="org-left">Unit</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Dw</code></td>
|
|
<td class="org-left">Ground motion</td>
|
|
<td class="org-right">3</td>
|
|
<td class="org-left">[m]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Fg</code></td>
|
|
<td class="org-left">External force applied on granite</td>
|
|
<td class="org-right">3</td>
|
|
<td class="org-left">[N]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Fs</code></td>
|
|
<td class="org-left">External force applied on the Sample</td>
|
|
<td class="org-right">3</td>
|
|
<td class="org-left">[N]</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4e58ed6" class="outline-4">
|
|
<h4 id="org4e58ed6"><span class="section-number-4">3.2.2</span> Measurement Noise</h4>
|
|
<div class="outline-text-4" id="text-3-2-2">
|
|
<table id="orgdadd8d8" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 5:</span> List of Measurement Noise</caption>
|
|
|
|
<colgroup>
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
</colgroup>
|
|
<thead>
|
|
<tr>
|
|
<th scope="col" class="org-left">Variable</th>
|
|
<th scope="col" class="org-left">Meaning</th>
|
|
<th scope="col" class="org-left">Size</th>
|
|
<th scope="col" class="org-left">Unit</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"> </td>
|
|
<td class="org-left"> </td>
|
|
<td class="org-left"> </td>
|
|
<td class="org-left"> </td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc9006ba" class="outline-4">
|
|
<h4 id="orgc9006ba"><span class="section-number-4">3.2.3</span> Control Inputs</h4>
|
|
<div class="outline-text-4" id="text-3-2-3">
|
|
<table id="orgfc73f52" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 6:</span> List of Control Inputs</caption>
|
|
|
|
<colgroup>
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
|
|
<col class="org-right" />
|
|
|
|
<col class="org-left" />
|
|
</colgroup>
|
|
<thead>
|
|
<tr>
|
|
<th scope="col" class="org-left">Variable</th>
|
|
<th scope="col" class="org-left">Meaning</th>
|
|
<th scope="col" class="org-right">Size</th>
|
|
<th scope="col" class="org-left">Unit</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Fy</code></td>
|
|
<td class="org-left">Actuation force for Ty</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[N]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Dy</code></td>
|
|
<td class="org-left">Imposed displacement for Ty</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[m]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>My</code></td>
|
|
<td class="org-left">Actuation torque for Ry</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[N.m]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Ry</code></td>
|
|
<td class="org-left">Imposed rotation for Ry</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Mz</code></td>
|
|
<td class="org-left">Actuation torque for Rz</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[N.m]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Rz</code></td>
|
|
<td class="org-left">Imposed rotation for Rz</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Fh</code></td>
|
|
<td class="org-left">Actuation force/torque for hexapod (cart)</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[N, N.m]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Fhl</code></td>
|
|
<td class="org-left">Actuation force/torque for hexapod (legs)</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[N]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Dh</code></td>
|
|
<td class="org-left">Imposed position for hexapod (cart)</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m, rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Rm</code></td>
|
|
<td class="org-left">Position of the two masses</td>
|
|
<td class="org-right">2</td>
|
|
<td class="org-left">[rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Fn</code></td>
|
|
<td class="org-left">Actuation force for the NASS (cart)</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[N, N.m]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Fnl</code></td>
|
|
<td class="org-left">Actuation force for the NASS's legs</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[N]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Dn</code></td>
|
|
<td class="org-left">Imposed position for the NASS (cart)</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m, rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgeb985b0" class="outline-3">
|
|
<h3 id="orgeb985b0"><span class="section-number-3">3.3</span> List of Outputs</h3>
|
|
<div class="outline-text-3" id="text-3-3">
|
|
<table id="org59a5204" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 7:</span> List of Outputs</caption>
|
|
|
|
<colgroup>
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
|
|
<col class="org-right" />
|
|
|
|
<col class="org-left" />
|
|
</colgroup>
|
|
<thead>
|
|
<tr>
|
|
<th scope="col" class="org-left">Variable</th>
|
|
<th scope="col" class="org-left">Meaning</th>
|
|
<th scope="col" class="org-right">Size</th>
|
|
<th scope="col" class="org-left">Unit</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Dgm</code></td>
|
|
<td class="org-left">Absolute displacement of the granite</td>
|
|
<td class="org-right">3</td>
|
|
<td class="org-left">[m]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Vgm</code></td>
|
|
<td class="org-left">Absolute Velocity of the granite</td>
|
|
<td class="org-right">3</td>
|
|
<td class="org-left">[m/s]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Dym</code></td>
|
|
<td class="org-left">Measured displacement of Ty</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[m]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Rym</code></td>
|
|
<td class="org-left">Measured rotation of Ry</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Rzm</code></td>
|
|
<td class="org-left">Measured rotation of Rz</td>
|
|
<td class="org-right">1</td>
|
|
<td class="org-left">[rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Dhm</code></td>
|
|
<td class="org-left">Measured position of hexapod (cart)</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m, rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Fnlm</code></td>
|
|
<td class="org-left">Measured force of NASS's legs</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[N]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Dnlm</code></td>
|
|
<td class="org-left">Measured elongation of NASS's legs</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Dnm</code></td>
|
|
<td class="org-left">Measured position of NASS w.r.t NASS's base</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m, rad]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Vnm</code></td>
|
|
<td class="org-left">Measured absolute velocity of NASS platform</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m/s, rad/s]</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"><code>Vnlm</code></td>
|
|
<td class="org-left">Measured absolute velocity of NASS's legs</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m/s]</td>
|
|
</tr>
|
|
</tbody>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left"><code>Dsm</code></td>
|
|
<td class="org-left">Position of Sample w.r.t. granite frame</td>
|
|
<td class="org-right">6</td>
|
|
<td class="org-left">[m, rad]</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfd5289f" class="outline-2">
|
|
<h2 id="orgfd5289f"><span class="section-number-2">4</span> Simulink Library</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
<p>
|
|
<a id="orgecb3837"></a>
|
|
</p>
|
|
|
|
<p>
|
|
A simulink library is developed in order to share elements between the different simulink files.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd1ebe0f" class="outline-3">
|
|
<h3 id="orgd1ebe0f"><span class="section-number-3">4.1</span> <code>inputs</code></h3>
|
|
<div class="outline-text-3" id="text-4-1">
|
|
<p>
|
|
<a href="../nass_library/inputs.slx">inputs.slx</a>
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1ae1d37" class="outline-3">
|
|
<h3 id="org1ae1d37"><span class="section-number-3">4.2</span> <code>nass_library</code></h3>
|
|
<div class="outline-text-3" id="text-4-2">
|
|
<p>
|
|
<a href="../nass_library/nass_library.slx">nass<sub>library.slx</sub></a>
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org54e3d07" class="outline-3">
|
|
<h3 id="org54e3d07"><span class="section-number-3">4.3</span> <code>pos_error_wrt_nass_base</code></h3>
|
|
<div class="outline-text-3" id="text-4-3">
|
|
<p>
|
|
<a href="../nass_library/pos_error_wrt_nass_base.slx">pos<sub>error</sub><sub>wrt</sub><sub>nass</sub><sub>base.slx</sub></a>
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc0f0f02" class="outline-3">
|
|
<h3 id="orgc0f0f02"><span class="section-number-3">4.4</span> <code>QuaternionToAngles</code></h3>
|
|
<div class="outline-text-3" id="text-4-4">
|
|
<p>
|
|
<a href="../nass_library/QuaternionToAngles.slx">QuaternionToAngles.slx</a>
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbda19ed" class="outline-3">
|
|
<h3 id="orgbda19ed"><span class="section-number-3">4.5</span> <code>RotationMatrixToAngle</code></h3>
|
|
<div class="outline-text-3" id="text-4-5">
|
|
<p>
|
|
<a href="../nass_library/RotationMatrixToAngle.slx">RotationMatrixToAngle.slx</a>
|
|
</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org54193c9" class="outline-2">
|
|
<h2 id="org54193c9"><span class="section-number-2">5</span> Functions</h2>
|
|
<div class="outline-text-2" id="text-5">
|
|
<p>
|
|
<a id="org94ca66a"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-orgc1f5840" class="outline-3">
|
|
<h3 id="orgc1f5840"><span class="section-number-3">5.1</span> computePsdDispl</h3>
|
|
<div class="outline-text-3" id="text-5-1">
|
|
<p>
|
|
<a id="org46da1ed"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computePsdDispl.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">psd_object</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">computePsdDispl</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">sys_data</span>, <span class="org-variable-name">t_init</span>, <span class="org-variable-name">n_av</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
i_init = find<span class="org-rainbow-delimiters-depth-1">(</span>sys_data.time <span class="org-type">></span> t_init, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
han_win = hanning<span class="org-rainbow-delimiters-depth-1">(</span>ceil<span class="org-rainbow-delimiters-depth-2">(</span>length<span class="org-rainbow-delimiters-depth-3">(</span>sys_data.Dx<span class="org-rainbow-delimiters-depth-4">(</span>i_init<span class="org-type">:</span>end, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">/</span>n_av<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
Fs = <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>sys_data.time<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>pdx, f<span class="org-rainbow-delimiters-depth-1">]</span> = pwelch<span class="org-rainbow-delimiters-depth-1">(</span>sys_data.Dx<span class="org-rainbow-delimiters-depth-2">(</span>i_init<span class="org-type">:</span>end, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, han_win, <span class="org-rainbow-delimiters-depth-2">[]</span>, <span class="org-rainbow-delimiters-depth-2">[]</span>, Fs<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>pdy, <span class="org-type">~</span><span class="org-rainbow-delimiters-depth-1">]</span> = pwelch<span class="org-rainbow-delimiters-depth-1">(</span>sys_data.Dy<span class="org-rainbow-delimiters-depth-2">(</span>i_init<span class="org-type">:</span>end, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, han_win, <span class="org-rainbow-delimiters-depth-2">[]</span>, <span class="org-rainbow-delimiters-depth-2">[]</span>, Fs<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>pdz, <span class="org-type">~</span><span class="org-rainbow-delimiters-depth-1">]</span> = pwelch<span class="org-rainbow-delimiters-depth-1">(</span>sys_data.Dz<span class="org-rainbow-delimiters-depth-2">(</span>i_init<span class="org-type">:</span>end, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, han_win, <span class="org-rainbow-delimiters-depth-2">[]</span>, <span class="org-rainbow-delimiters-depth-2">[]</span>, Fs<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>prx, <span class="org-type">~</span><span class="org-rainbow-delimiters-depth-1">]</span> = pwelch<span class="org-rainbow-delimiters-depth-1">(</span>sys_data.Rx<span class="org-rainbow-delimiters-depth-2">(</span>i_init<span class="org-type">:</span>end, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, han_win, <span class="org-rainbow-delimiters-depth-2">[]</span>, <span class="org-rainbow-delimiters-depth-2">[]</span>, Fs<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>pry, <span class="org-type">~</span><span class="org-rainbow-delimiters-depth-1">]</span> = pwelch<span class="org-rainbow-delimiters-depth-1">(</span>sys_data.Ry<span class="org-rainbow-delimiters-depth-2">(</span>i_init<span class="org-type">:</span>end, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, han_win, <span class="org-rainbow-delimiters-depth-2">[]</span>, <span class="org-rainbow-delimiters-depth-2">[]</span>, Fs<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>prz, <span class="org-type">~</span><span class="org-rainbow-delimiters-depth-1">]</span> = pwelch<span class="org-rainbow-delimiters-depth-1">(</span>sys_data.Rz<span class="org-rainbow-delimiters-depth-2">(</span>i_init<span class="org-type">:</span>end, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, han_win, <span class="org-rainbow-delimiters-depth-2">[]</span>, <span class="org-rainbow-delimiters-depth-2">[]</span>, Fs<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
psd_object = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
|
<span class="org-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, ...
|
|
<span class="org-string">'rz'</span>, prz<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0d8e9e6" class="outline-3">
|
|
<h3 id="org0d8e9e6"><span class="section-number-3">5.2</span> computeSetpoint</h3>
|
|
<div class="outline-text-3" id="text-5-2">
|
|
<p>
|
|
<a id="orga7c431b"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/computeSetpoint.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">setpoint</span> = <span class="org-function-name">computeSetpoint</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">ty</span>, <span class="org-variable-name">ry</span>, <span class="org-variable-name">rz</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
setpoint = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Ty</span></span>
|
|
Ty = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ty ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
% Tyinv = [<span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
% <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-type">-</span>ty ;
|
|
% <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
% <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> ];
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Ry</span></span>
|
|
Ry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-comment">% TMry = Ty*Ry*Tyinv;</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Rz</span></span>
|
|
Rz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
sin<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-comment">% TMrz = Ty*TMry*Rz*TMry'*Tyinv;</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% All stages</span></span>
|
|
<span class="org-comment">% </span><span class="org-comment"><span class="org-constant">TM </span></span><span class="org-comment">= TMrz*TMry*Ty;</span>
|
|
|
|
TM = Ty<span class="org-type">*</span>Ry<span class="org-type">*</span>Rz;
|
|
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>thetax, thetay, thetaz<span class="org-rainbow-delimiters-depth-1">]</span> = RM2angle<span class="org-rainbow-delimiters-depth-1">(</span>TM<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
setpoint<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = TM<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
setpoint<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>thetax, thetay, thetaz<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Custom Functions</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">thetax, thetay, thetaz</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">RM2angle</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">R</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">if</span> abs<span class="org-rainbow-delimiters-depth-1">(</span>abs<span class="org-rainbow-delimiters-depth-2">(</span>R<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">></span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> % R31 <span class="org-type">!</span>= <span class="org-highlight-numbers-number">1</span> and R31 <span class="org-type">!</span>= <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>
|
|
thetay = <span class="org-type">-</span>asin<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
thetax = atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
thetaz = atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
thetaz = <span class="org-highlight-numbers-number">0</span>;
|
|
<span class="org-keyword">if</span> abs<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type"><</span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> % R31 = <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>
|
|
thetay = <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>;
|
|
thetax = thetaz <span class="org-type">+</span> atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
thetay = <span class="org-type">-</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>;
|
|
thetax = <span class="org-type">-</span>thetaz <span class="org-type">+</span> atan2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">-</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">-</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9a191e3" class="outline-3">
|
|
<h3 id="org9a191e3"><span class="section-number-3">5.3</span> converErrorBasis</h3>
|
|
<div class="outline-text-3" id="text-5-3">
|
|
<p>
|
|
<a id="org2e6fbe8"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/converErrorBasis.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">error_nass</span> = <span class="org-function-name">convertErrorBasis</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">pos</span>, <span class="org-variable-name">setpoint</span>, <span class="org-variable-name">ty</span>, <span class="org-variable-name">ry</span>, <span class="org-variable-name">rz</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-comment">% convertErrorBasis -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: convertErrorBasis(p_error, ty, ry, rz)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - p_error - Position error of the sample w.r.t. the granite [m, rad]</span>
|
|
<span class="org-comment">% - ty - Measured translation of the Ty stage [m]</span>
|
|
<span class="org-comment">% - ry - Measured rotation of the Ry stage [rad]</span>
|
|
<span class="org-comment">% - rz - Measured rotation of the Rz stage [rad]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - P_nass - Position error of the sample w.r.t. the NASS base [m]</span>
|
|
<span class="org-comment">% - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Example:</span>
|
|
<span class="org-comment">%</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% If line vector => column vector</span></span>
|
|
<span class="org-keyword">if</span> size<span class="org-rainbow-delimiters-depth-1">(</span>pos, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">==</span> <span class="org-highlight-numbers-number">6</span>
|
|
pos = pos<span class="org-type">'</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">if</span> size<span class="org-rainbow-delimiters-depth-1">(</span>setpoint, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">==</span> <span class="org-highlight-numbers-number">6</span>
|
|
setpoint = setpoint<span class="org-type">'</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Position of the sample in the frame fixed to the Granite</span></span>
|
|
P_granite = <span class="org-rainbow-delimiters-depth-1">[</span>pos<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% Position [m]</span>
|
|
R_granite = <span class="org-rainbow-delimiters-depth-1">[</span>setpoint<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% Reference [m]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Transformation matrices of the stages</span></span>
|
|
<span class="org-comment">% T-y</span>
|
|
TMty = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ty ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-comment">% R-y</span>
|
|
TMry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-comment">% R-z</span>
|
|
TMrz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
sin<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Compute Point coordinates in the new reference fixed to the NASS base</span></span>
|
|
<span class="org-comment">% P_nass = TMrz*TMry*TMty*P_granite;</span>
|
|
P_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span>P_granite;
|
|
R_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span>R_granite;
|
|
|
|
dx = R_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>P_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
dy = R_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>P_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
dz = R_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>P_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Compute new basis vectors linked to the NASS base</span></span>
|
|
% ux_nass = TMrz<span class="org-type">*</span>TMry<span class="org-type">*</span>TMty<span class="org-type">*</span>[<span class="org-highlight-numbers-number">1</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>];
|
|
% ux_nass = ux_nass(<span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>);
|
|
% uy_nass = TMrz<span class="org-type">*</span>TMry<span class="org-type">*</span>TMty<span class="org-type">*</span>[<span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">1</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>];
|
|
% uy_nass = uy_nass(<span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>);
|
|
% uz_nass = TMrz<span class="org-type">*</span>TMry<span class="org-type">*</span>TMty<span class="org-type">*</span>[<span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">1</span>; <span class="org-highlight-numbers-number">0</span>];
|
|
% uz_nass = uz_nass(<span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>);
|
|
|
|
ux_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ux_nass = ux_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
uy_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">1</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
uy_nass = uy_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
uz_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">1</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
uz_nass = uz_nass<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Rotations error w.r.t. granite Frame</span></span>
|
|
<span class="org-comment">% Rotations error w.r.t. granite Frame</span>
|
|
rx_nass = pos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
ry_nass = pos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
rz_nass = pos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-comment">% Rotation matrices of the Sample w.r.t. the Granite</span>
|
|
Mrx_error = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rx_nass<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rx_nass<span class="org-rainbow-delimiters-depth-2">)</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rx_nass<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rx_nass<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
Mry_error = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>ry_nass<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>ry_nass<span class="org-rainbow-delimiters-depth-2">)</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>ry_nass<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>ry_nass<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
Mrz_error = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rz_nass<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rz_nass<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
sin<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rz_nass<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">-</span>rz_nass<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-comment">% Rotation matrix of the Sample w.r.t. the Granite</span>
|
|
Mr_error = Mrz_error<span class="org-type">*</span>Mry_error<span class="org-type">*</span>Mrx_error;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Use matrix to solve</span></span>
|
|
R = Mr_error<span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">[</span>ux_nass, uy_nass, uz_nass<span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% Rotation matrix from NASS base to Sample</span>
|
|
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>thetax, thetay, thetaz<span class="org-rainbow-delimiters-depth-1">]</span> = RM2angle<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
error_nass = <span class="org-rainbow-delimiters-depth-1">[</span>dx; dy; dz; thetax; thetay; thetaz<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Custom Functions</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">thetax, thetay, thetaz</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">RM2angle</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">R</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">if</span> abs<span class="org-rainbow-delimiters-depth-1">(</span>abs<span class="org-rainbow-delimiters-depth-2">(</span>R<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">></span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> % R31 <span class="org-type">!</span>= <span class="org-highlight-numbers-number">1</span> and R31 <span class="org-type">!</span>= <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>
|
|
thetay = <span class="org-type">-</span>asin<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% thetaybis = pi-thetay;</span>
|
|
thetax = atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
% thetaxbis = atan2(R(<span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">2</span>)<span class="org-type">/</span>cos(thetaybis), R(<span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">3</span>)<span class="org-type">/</span>cos(thetaybis));
|
|
thetaz = atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
% thetazbis = atan2(R(<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">1</span>)<span class="org-type">/</span>cos(thetaybis), R(<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>)<span class="org-type">/</span>cos(thetaybis));
|
|
<span class="org-keyword">else</span>
|
|
thetaz = <span class="org-highlight-numbers-number">0</span>;
|
|
<span class="org-keyword">if</span> abs<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type"><</span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> % R31 = <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>
|
|
thetay = <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>;
|
|
thetax = thetaz <span class="org-type">+</span> atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
thetay = <span class="org-type">-</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>;
|
|
thetax = <span class="org-type">-</span>thetaz <span class="org-type">+</span> atan2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">-</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">-</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2d44efa" class="outline-3">
|
|
<h3 id="org2d44efa"><span class="section-number-3">5.4</span> generateDiagPidControl</h3>
|
|
<div class="outline-text-3" id="text-5-4">
|
|
<p>
|
|
<a id="orgd1ce773"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/generateDiagPidControl.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">K</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">generateDiagPidControl</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">G</span>, <span class="org-variable-name">fs</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
pid_opts = pidtuneOptions<span class="org-rainbow-delimiters-depth-1">(</span>...
|
|
<span class="org-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>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
K = tf<span class="org-rainbow-delimiters-depth-1">(</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
input_name = G.InputName<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
output_name = G.OutputName<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
K<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = tf<span class="org-rainbow-delimiters-depth-1">(</span>pidtune<span class="org-rainbow-delimiters-depth-2">(</span>minreal<span class="org-rainbow-delimiters-depth-3">(</span>G<span class="org-rainbow-delimiters-depth-4">(</span>output_name, input_name<span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span>, <span class="org-string">'PIDF'</span>, <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>fs, pid_opts<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
K.InputName = G.OutputName;
|
|
K.OutputName = G.InputName;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="outline-container-org6f7736a" class="outline-3">
|
|
<h3 id="org6f7736a"><span class="section-number-3">5.5</span> identifyPlant</h3>
|
|
<div class="outline-text-3" id="text-5-5">
|
|
<p>
|
|
<a id="orgd69607b"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/identifyPlant.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">sys</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">identifyPlant</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Options for Linearized</span></span>
|
|
options = linearizeOptions;
|
|
options.SampleTime = <span class="org-highlight-numbers-number">0</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'sim_nano_station_id'</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
|
|
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> = linio<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>mdl, <span class="org-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">'/Fd'</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">% Disturbance Forces</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">'/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">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">'/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">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">'/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">9</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>
|
|
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">10</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">'/Vlm'</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">% Measured absolute velocity of the legs</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G = linearize<span class="org-rainbow-delimiters-depth-1">(</span>mdl, io, options<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
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-string">'Frzz'</span>, <span class="org-string">'Ftyx'</span>, <span class="org-string">'Ftyz'</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">'Edy'</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-string">'Vm1'</span>, <span class="org-string">'Vm2'</span>, <span class="org-string">'Vm3'</span>, <span class="org-string">'Vm4'</span>, <span class="org-string">'Vm5'</span>, <span class="org-string">'Vm6'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Create the sub transfer functions</span></span>
|
|
minreal_tol = sqrt<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From forces applied in the cartesian frame to displacement of the sample in the cartesian frame</span>
|
|
sys.G_cart = minreal<span class="org-rainbow-delimiters-depth-1">(</span>G<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">{</span><span class="org-string">'Dx'</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>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From ground motion to Sample displacement</span>
|
|
sys.G_gm = minreal<span class="org-rainbow-delimiters-depth-1">(</span>G<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">{</span><span class="org-string">'Dx'</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>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From direct forces applied on the sample to displacement of the sample</span>
|
|
sys.G_fs = minreal<span class="org-rainbow-delimiters-depth-1">(</span>G<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">{</span><span class="org-string">'Dx'</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>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From forces applied on NASS's legs to force sensor in each leg</span>
|
|
sys.G_iff = minreal<span class="org-rainbow-delimiters-depth-1">(</span>G<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">{</span><span class="org-string">'Fm1'</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>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From forces applied on NASS's legs to displacement of each leg</span>
|
|
sys.G_dleg = minreal<span class="org-rainbow-delimiters-depth-1">(</span>G<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">{</span><span class="org-string">'Dm1'</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>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From forces/torques applied by the NASS to position error</span>
|
|
sys.G_plant = minreal<span class="org-rainbow-delimiters-depth-1">(</span>G<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-rainbow-delimiters-depth-3">{</span><span class="org-string">'Edx'</span>, <span class="org-string">'Edy'</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>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From forces/torques applied by the NASS to velocity of the actuator</span>
|
|
sys.G_geoph = 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">'Vm1'</span>, <span class="org-string">'Vm2'</span>, <span class="org-string">'Vm3'</span>, <span class="org-string">'Vm4'</span>, <span class="org-string">'Vm5'</span>, <span class="org-string">'Vm6'</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>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-comment">% From various disturbance forces to position error</span>
|
|
sys.G_dist = 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">'Frzz'</span>, <span class="org-string">'Ftyx'</span>, <span class="org-string">'Ftyz'</span><span class="org-rainbow-delimiters-depth-3">}</span><span class="org-rainbow-delimiters-depth-2">)</span>, minreal_tol, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% We remove low frequency and high frequency dynamics that are usually unstable</span></span>
|
|
<span class="org-comment">% using =freqsep= is risky as it may change the shape of the transfer functions</span>
|
|
% f_min = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span>; % [Hz]
|
|
% f_max = <span class="org-highlight-numbers-number">1e4</span>; % [Hz]
|
|
|
|
% [<span class="org-type">~</span>, sys.G_cart] = freqsep(freqsep(sys.G_cart, <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_max), <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_min);
|
|
% [<span class="org-type">~</span>, sys.G_gm] = freqsep(freqsep(sys.G_gm, <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_max), <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_min);
|
|
% [<span class="org-type">~</span>, sys.G_fs] = freqsep(freqsep(sys.G_fs, <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_max), <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_min);
|
|
% [<span class="org-type">~</span>, sys.G_iff] = freqsep(freqsep(sys.G_iff, <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_max), <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_min);
|
|
% [<span class="org-type">~</span>, sys.G_dleg] = freqsep(freqsep(sys.G_dleg, <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_max), <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_min);
|
|
% [<span class="org-type">~</span>, sys.G_plant] = freqsep(freqsep(sys.G_plant, <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_max), <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>f_min);
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% We finally verify that the system is stable</span></span>
|
|
<span class="org-keyword">if</span> <span class="org-type">~</span>isstable<span class="org-rainbow-delimiters-depth-1">(</span>sys.G_cart<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">||</span> <span class="org-type">~</span>isstable<span class="org-rainbow-delimiters-depth-1">(</span>sys.G_gm<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">||</span> <span class="org-type">~</span>isstable<span class="org-rainbow-delimiters-depth-1">(</span>sys.G_fs<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">||</span> <span class="org-type">~</span>isstable<span class="org-rainbow-delimiters-depth-1">(</span>sys.G_iff<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">||</span> <span class="org-type">~</span>isstable<span class="org-rainbow-delimiters-depth-1">(</span>sys.G_dleg<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">||</span> <span class="org-type">~</span>isstable<span class="org-rainbow-delimiters-depth-1">(</span>sys.G_plant<span class="org-rainbow-delimiters-depth-1">)</span>
|
|
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'One of the identified system is unstable'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org32dd782" class="outline-3">
|
|
<h3 id="org32dd782"><span class="section-number-3">5.6</span> runSimulation</h3>
|
|
<div class="outline-text-3" id="text-5-6">
|
|
<p>
|
|
<a id="org95edf73"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/runSimulation.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[]</span></span> = <span class="org-function-name">runSimulation</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">sys_name</span>, <span class="org-variable-name">sys_mass</span>, <span class="org-variable-name">ctrl_type</span>, <span class="org-variable-name">act_damp</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Load the controller and save it for the simulation</span></span>
|
|
<span class="org-keyword">if</span> strcmp<span class="org-rainbow-delimiters-depth-1">(</span>ctrl_type, <span class="org-string">'cl'</span><span class="org-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>
|
|
K_obj = load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/K_fb.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
K = K_obj.<span class="org-rainbow-delimiters-depth-1">(</span>sprintf<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'K_%s_%s'</span>, sys_mass, sys_name)); <span class="org-comment">%#</span><span class="org-comment"><span class="org-bold">ok</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-string">'./mat/controllers.mat'</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>
|
|
K_obj = load<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-string">'./mat/K_fb_iff.mat'</span><span class="org-rainbow-delimiters-depth-3">)</span>;
|
|
K = K_obj.<span class="org-rainbow-delimiters-depth-3">(</span>sprintf<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-string">'K_%s_%s_iff'</span>, sys_mass, sys_name)); <span class="org-comment">%#</span><span class="org-comment"><span class="org-bold">ok</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-5">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span><span class="org-rainbow-delimiters-depth-5">)</span>;
|
|
<span class="org-keyword">elseif</span> strcmp<span class="org-rainbow-delimiters-depth-5">(</span>ctrl_type, <span class="org-string">'ol'</span><span class="org-rainbow-delimiters-depth-5">)</span>
|
|
K = tf<span class="org-rainbow-delimiters-depth-5">(</span>zeros<span class="org-rainbow-delimiters-depth-6">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-6">)</span><span class="org-rainbow-delimiters-depth-5">)</span>; <span class="org-comment">%#</span><span class="org-comment"><span class="org-bold">ok</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-5">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K'</span><span class="org-rainbow-delimiters-depth-5">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
error<span class="org-rainbow-delimiters-depth-5">(</span><span class="org-string">'ctrl_type should be cl or ol'</span><span class="org-rainbow-delimiters-depth-5">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Active Damping</span></span>
|
|
<span class="org-keyword">if</span> strcmp<span class="org-rainbow-delimiters-depth-5">(</span>act_damp, <span class="org-string">'iff'</span><span class="org-rainbow-delimiters-depth-5">)</span>
|
|
K_iff_crit = load<span class="org-rainbow-delimiters-depth-5">(</span><span class="org-string">'./mat/K_iff_crit.mat'</span><span class="org-rainbow-delimiters-depth-5">)</span>;
|
|
K_iff = K_iff_crit.<span class="org-rainbow-delimiters-depth-5">(</span>sprintf<span class="org-rainbow-delimiters-depth-6">(</span><span class="org-string">'K_iff_%s_%s'</span>, sys_mass, sys_name)); <span class="org-comment">%#</span><span class="org-comment"><span class="org-bold">ok</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-7">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">elseif</span> strcmp<span class="org-rainbow-delimiters-depth-7">(</span>act_damp, <span class="org-string">'none'</span><span class="org-rainbow-delimiters-depth-7">)</span>
|
|
K_iff = tf<span class="org-rainbow-delimiters-depth-7">(</span>zeros<span class="org-rainbow-delimiters-depth-8">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-8">)</span><span class="org-rainbow-delimiters-depth-7">)</span>; <span class="org-comment">%#</span><span class="org-comment"><span class="org-bold">ok</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-7">(</span><span class="org-string">'./mat/controllers.mat'</span>, <span class="org-string">'K_iff'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
<span class="org-keyword">if</span> strcmp<span class="org-rainbow-delimiters-depth-7">(</span>sys_name, <span class="org-string">'pz'</span><span class="org-rainbow-delimiters-depth-7">)</span>
|
|
initializeNanoHexapod<span class="org-rainbow-delimiters-depth-7">(</span>struct<span class="org-rainbow-delimiters-depth-8">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-8">)</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">elseif</span> strcmp<span class="org-rainbow-delimiters-depth-7">(</span>sys_name, <span class="org-string">'vc'</span><span class="org-rainbow-delimiters-depth-7">)</span>
|
|
initializeNanoHexapod<span class="org-rainbow-delimiters-depth-7">(</span>struct<span class="org-rainbow-delimiters-depth-8">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'lorentz'</span><span class="org-rainbow-delimiters-depth-8">)</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
error<span class="org-rainbow-delimiters-depth-7">(</span><span class="org-string">'sys_name should be pz or vc'</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">if</span> strcmp<span class="org-rainbow-delimiters-depth-7">(</span>sys_mass, <span class="org-string">'light'</span><span class="org-rainbow-delimiters-depth-7">)</span>
|
|
initializeSample<span class="org-rainbow-delimiters-depth-7">(</span>struct<span class="org-rainbow-delimiters-depth-8">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-8">)</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">elseif</span> strcmp<span class="org-rainbow-delimiters-depth-7">(</span>sys_mass, <span class="org-string">'heavy'</span><span class="org-rainbow-delimiters-depth-7">)</span>
|
|
initializeSample<span class="org-rainbow-delimiters-depth-7">(</span>struct<span class="org-rainbow-delimiters-depth-8">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-8">)</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
error<span class="org-rainbow-delimiters-depth-7">(</span><span class="org-string">'sys_mass should be light or heavy'</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the simulation</span></span>
|
|
<span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-7">(</span><span class="org-string">'sim_nano_station_ctrl.slx'</span><span class="org-rainbow-delimiters-depth-7">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Split the Dsample matrix into vectors</span></span>
|
|
<span class="org-rainbow-delimiters-depth-7">[</span>Dx, Dy, Dz, Rx, Ry, Rz<span class="org-rainbow-delimiters-depth-7">]</span> = matSplit<span class="org-rainbow-delimiters-depth-7">(</span>Es.Data, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-7">)</span>; <span class="org-comment">%#</span><span class="org-comment"><span class="org-bold">ok</span></span>
|
|
time = Dsample.Time; <span class="org-comment">%#</span><span class="org-comment"><span class="org-bold">ok</span></span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save the result</span></span>
|
|
filename = sprintf<span class="org-rainbow-delimiters-depth-7">(</span><span class="org-string">'sim_%s_%s_%s_%s'</span>, sys_mass, sys_name, ctrl_type, act_damp);
|
|
save<span class="org-rainbow-delimiters-depth-8">(</span>sprintf<span class="org-rainbow-delimiters-depth-9">(</span><span class="org-string">'./mat/%s.mat'</span>, filename), ...
|
|
<span class="org-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>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="outline-container-org5cc2276" class="outline-2">
|
|
<h2 id="org5cc2276"><span class="section-number-2">6</span> Initialize Elements</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="org18b2c56"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org664a7e3" class="outline-3">
|
|
<h3 id="org664a7e3"><span class="section-number-3">6.1</span> Experiment</h3>
|
|
<div class="outline-text-3" id="text-6-1">
|
|
<p>
|
|
<a id="org395aa48"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeExperiment.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[]</span></span> = <span class="org-function-name">initializeExperiment</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">exp_name</span>, <span class="org-variable-name">sys_mass</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">if</span> strcmp<span class="org-rainbow-delimiters-depth-1">(</span>exp_name, <span class="org-string">'tomography'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">if</span> strcmp<span class="org-rainbow-delimiters-depth-1">(</span>sys_mass, <span class="org-string">'light'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
opts_inputs = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
|
<span class="org-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>
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> strcpm<span class="org-rainbow-delimiters-depth-1">(</span>sys_mass, <span class="org-string">'heavy'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
opts_inputs = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
|
<span class="org-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>
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
error<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'sys_mass should be light or heavy'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
initializeInputs<span class="org-rainbow-delimiters-depth-1">(</span>opts_inputs<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
error<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'exp_name is only configured for tomography'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org89c58f8" class="outline-3">
|
|
<h3 id="org89c58f8"><span class="section-number-3">6.2</span> Generate Reference Signals</h3>
|
|
<div class="outline-text-3" id="text-6-2">
|
|
<p>
|
|
<a id="orgdda511e"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">ref</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeReferences</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span> ...
|
|
<span class="org-string">'Ts'</span>, <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>, ...<span class="org-comment"> % Sampling Frequency [s]</span>
|
|
<span class="org-string">'Dy_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
|
|
<span class="org-string">'Dy_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude of the displacement [m]</span>
|
|
<span class="org-string">'Dy_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
|
|
<span class="org-string">'Ry_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
|
|
<span class="org-string">'Ry_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude [rad]</span>
|
|
<span class="org-string">'Ry_period'</span>, <span class="org-highlight-numbers-number">10</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
|
|
<span class="org-string">'Rz_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "rotating"</span>
|
|
<span class="org-string">'Rz_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Initial angle [rad]</span>
|
|
<span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the rotating [s]</span>
|
|
<span class="org-string">'Dh_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
|
|
<span class="org-string">'Dh_pos'</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">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-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
|
|
<span class="org-string">'Rm_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
|
|
<span class="org-string">'Rm_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-constant">pi</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position of the two masses</span>
|
|
<span class="org-string">'Dn_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
|
|
<span class="org-string">'Dn_pos'</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">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-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Set Sampling Time</span></span>
|
|
Ts = opts.Ts;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Translation stage - Dy</span></span>
|
|
t = <span class="org-highlight-numbers-number">0</span><span class="org-type">:</span>Ts<span class="org-type">:</span>opts.Dy_period<span class="org-type">-</span>Ts; <span class="org-comment">% Time Vector [s]</span>
|
|
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">switch</span> <span class="org-constant">opts.Dy_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Dy<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Dy_amplitude;
|
|
<span class="org-keyword">case</span> <span class="org-string">'triangular'</span>
|
|
Dy<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= <span class="org-type">-</span><span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Dy_amplitude <span class="org-type">+</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Dy_amplitude<span class="org-type">/</span>opts.Dy_period<span class="org-type">*</span>t;
|
|
Dy<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Dy_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>opts.Dy_amplitude <span class="org-type">-</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Dy_amplitude<span class="org-type">/</span>opts.Dy_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Dy_period<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
Dy<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Dy_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Dy_amplitude<span class="org-type">/</span>opts.Dy_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Dy_period<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span>
|
|
Dy<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Dy_amplitude<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>opts.Dy_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">otherwise</span>
|
|
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Dy_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
Dy = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'values'</span>, Dy<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">%% Tilt Stage - Ry</span></span>
|
|
t = <span class="org-highlight-numbers-number">0</span><span class="org-type">:</span>Ts<span class="org-type">:</span>opts.Ry_period<span class="org-type">-</span>Ts;
|
|
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">switch</span> <span class="org-constant">opts.Ry_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Ry<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Ry_amplitude;
|
|
<span class="org-keyword">case</span> <span class="org-string">'triangular'</span>
|
|
Ry<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= <span class="org-type">-</span><span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude <span class="org-type">+</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t;
|
|
Ry<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>opts.Ry_amplitude <span class="org-type">-</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
Ry<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span>
|
|
|
|
<span class="org-keyword">otherwise</span>
|
|
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Ry_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
Ry = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'values'</span>, Ry<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">%% Spindle - Rz</span></span>
|
|
t = <span class="org-highlight-numbers-number">0</span><span class="org-type">:</span>Ts<span class="org-type">:</span>opts.Rz_period<span class="org-type">-</span>Ts;
|
|
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">switch</span> <span class="org-constant">opts.Rz_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Rz<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Rz_amplitude;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rotating'</span>
|
|
Rz<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Rz_amplitude<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>opts.Rz_period<span class="org-type">*</span>t;
|
|
<span class="org-keyword">otherwise</span>
|
|
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Rz_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
Rz = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'values'</span>, Rz<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">%% Micro-Hexapod</span></span>
|
|
t = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>, Ts<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
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">switch</span> <span class="org-constant">opts.Dh_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Dh = <span class="org-rainbow-delimiters-depth-1">[</span>opts.Dh_pos, opts.Dh_pos<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
<span class="org-keyword">otherwise</span>
|
|
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Dh_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
Dh = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'values'</span>, Dh<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">%% Axis Compensation - Rm</span></span>
|
|
t = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>, Ts<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
Rm = <span class="org-rainbow-delimiters-depth-1">[</span>opts.Rm_pos, opts.Rm_pos<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
Rm = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'values'</span>, Rm<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">%% Nano-Hexapod</span></span>
|
|
t = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>, Ts<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
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">switch</span> <span class="org-constant">opts.Dn_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Dn = <span class="org-rainbow-delimiters-depth-1">[</span>opts.Dn_pos, opts.Dn_pos<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
<span class="org-keyword">otherwise</span>
|
|
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Dn_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
Dn = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'values'</span>, Dn<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">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/nass_references.mat'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>, <span class="org-string">'Dh'</span>, <span class="org-string">'Rm'</span>, <span class="org-string">'Dn'</span>, <span class="org-string">'Ts'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org77d160f" class="outline-3">
|
|
<h3 id="org77d160f"><span class="section-number-3">6.3</span> <span class="todo TODO">TODO</span> Inputs</h3>
|
|
<div class="outline-text-3" id="text-6-3">
|
|
<p>
|
|
<a id="org7501b33"></a>
|
|
</p>
|
|
|
|
<ul class="org-ul">
|
|
<li class="off"><code>[ ]</code> <b>This function should not be used anymore</b>. Now there are two functions to initialize disturbances and references.</li>
|
|
</ul>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">inputs</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeInputs</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span> ...
|
|
<span class="org-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> ...
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Load Sampling Time and Simulation Time</span></span>
|
|
load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/sim_conf.mat'</span>, <span class="org-string">'sim_conf'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Define the time vector</span></span>
|
|
t = <span class="org-highlight-numbers-number">0</span><span class="org-type">:</span>sim_conf.Ts<span class="org-type">:</span>sim_conf.Tsim;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Ground motion - Dw</span></span>
|
|
<span class="org-keyword">if</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dw<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dw <span class="org-type">==</span> <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>;
|
|
Dw = <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-highlight-numbers-number">100</span><span class="org-type">*</span>random<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'norm'</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
Dw<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> = lsim<span class="org-rainbow-delimiters-depth-1">(</span>Wxg, Dw<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, t<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
Dw<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span> = lsim<span class="org-rainbow-delimiters-depth-1">(</span>Wxg, Dw<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, t<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
Dw<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = lsim<span class="org-rainbow-delimiters-depth-1">(</span>Wxg, Dw<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>, t<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dw<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dw <span class="org-type">==</span> <span class="org-constant">false</span>
|
|
Dw = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
Dw = opts.Dw;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Translation stage - Dy</span></span>
|
|
<span class="org-keyword">if</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dy<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dy <span class="org-type">==</span> <span class="org-constant">true</span>
|
|
Dy = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dy<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dy <span class="org-type">==</span> <span class="org-constant">false</span>
|
|
Dy = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
Dy = opts.Dy;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Tilt Stage - Ry</span></span>
|
|
<span class="org-keyword">if</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Ry<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Ry <span class="org-type">==</span> <span class="org-constant">true</span>
|
|
Ry = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">360</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Ry<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Ry <span class="org-type">==</span> <span class="org-constant">false</span>
|
|
Ry = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> isnumeric<span class="org-rainbow-delimiters-depth-1">(</span>opts.Ry<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> length<span class="org-rainbow-delimiters-depth-1">(</span>opts.Ry<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">==</span> <span class="org-highlight-numbers-number">1</span>
|
|
Ry = opts.Ry<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">360</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span>ones<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
Ry = opts.Ry;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Spindle - Rz</span></span>
|
|
<span class="org-keyword">if</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Rz<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Rz <span class="org-type">==</span> <span class="org-constant">true</span>
|
|
Rz = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">*</span>t;
|
|
<span class="org-keyword">elseif</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Rz<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Rz <span class="org-type">==</span> <span class="org-constant">false</span>
|
|
Rz = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> isnumeric<span class="org-rainbow-delimiters-depth-1">(</span>opts.Rz<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> length<span class="org-rainbow-delimiters-depth-1">(</span>opts.Rz<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">==</span> <span class="org-highlight-numbers-number">1</span>
|
|
Rz = opts.Rz<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">60</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span>t;
|
|
<span class="org-keyword">else</span>
|
|
Rz = opts.Rz;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Micro Hexapod - Dh</span></span>
|
|
<span class="org-keyword">if</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dh<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dh <span class="org-type">==</span> <span class="org-constant">true</span>
|
|
Dh = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dh<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dh <span class="org-type">==</span> <span class="org-constant">false</span>
|
|
Dh = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
Dh = opts.Dh;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Axis Compensation - Rm</span></span>
|
|
<span class="org-keyword">if</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Rm<span class="org-rainbow-delimiters-depth-1">)</span>
|
|
Rm = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
Rm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-constant">pi</span><span class="org-type">*</span>ones<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
Rm = opts.Rm;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Nano Hexapod - Dn</span></span>
|
|
<span class="org-keyword">if</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dn<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dn <span class="org-type">==</span> <span class="org-constant">true</span>
|
|
Dn = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">elseif</span> islogical<span class="org-rainbow-delimiters-depth-1">(</span>opts.Dn<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">&&</span> opts.Dn <span class="org-type">==</span> <span class="org-constant">false</span>
|
|
Dn = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
Dn = opts.Dn;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Setpoint - Ds</span></span>
|
|
Ds = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
Ds<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = computeSetpoint<span class="org-rainbow-delimiters-depth-1">(</span>Dy<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Ry<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Rz<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% External Forces applied on the Granite</span></span>
|
|
Fg = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% External Forces applied on the Sample</span></span>
|
|
Fs = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Create the input Structure that will contain all the inputs</span></span>
|
|
inputs = struct<span class="org-rainbow-delimiters-depth-1">(</span> ...
|
|
<span class="org-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> ...
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/inputs.mat'</span>, <span class="org-string">'inputs'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Custom Functions</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name">setpoint</span> = <span class="org-function-name">computeSetpoint</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">ty</span>, <span class="org-variable-name">ry</span>, <span class="org-variable-name">rz</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
setpoint = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Ty</span></span>
|
|
TMTy = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ty ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Ry</span></span>
|
|
TMRy = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Rz</span></span>
|
|
TMRz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
sin<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% All stages</span></span>
|
|
TM = TMTy<span class="org-type">*</span>TMRy<span class="org-type">*</span>TMRz;
|
|
|
|
<span class="org-rainbow-delimiters-depth-1">[</span>thetax, thetay, thetaz<span class="org-rainbow-delimiters-depth-1">]</span> = RM2angle<span class="org-rainbow-delimiters-depth-1">(</span>TM<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
setpoint<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = TM<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
setpoint<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>thetax, thetay, thetaz<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Custom Functions</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">thetax, thetay, thetaz</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">RM2angle</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">R</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">if</span> abs<span class="org-rainbow-delimiters-depth-1">(</span>abs<span class="org-rainbow-delimiters-depth-2">(</span>R<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">></span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> % R31 <span class="org-type">!</span>= <span class="org-highlight-numbers-number">1</span> and R31 <span class="org-type">!</span>= <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>
|
|
thetay = <span class="org-type">-</span>asin<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
thetax = atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
thetaz = atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>thetay<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
thetaz = <span class="org-highlight-numbers-number">0</span>;
|
|
<span class="org-keyword">if</span> abs<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">+</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type"><</span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> % R31 = <span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>
|
|
thetay = <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>;
|
|
thetax = thetaz <span class="org-type">+</span> atan2<span class="org-rainbow-delimiters-depth-1">(</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
thetay = <span class="org-type">-</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>;
|
|
thetax = <span class="org-type">-</span>thetaz <span class="org-type">+</span> atan2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">-</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">-</span>R<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org962187e" class="outline-3">
|
|
<h3 id="org962187e"><span class="section-number-3">6.4</span> Ground</h3>
|
|
<div class="outline-text-3" id="text-6-4">
|
|
<p>
|
|
<a id="org763b0ff"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeGround.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">ground</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeGround</span><span class="org-rainbow-delimiters-depth-1">()</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
ground = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
|
|
ground.shape = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
|
|
ground.density = <span class="org-highlight-numbers-number">2800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ground.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'ground'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org198c366" class="outline-3">
|
|
<h3 id="org198c366"><span class="section-number-3">6.5</span> Granite</h3>
|
|
<div class="outline-text-3" id="text-6-5">
|
|
<p>
|
|
<a id="orgbdb5477"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeGranite.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">granite</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeGranite</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rigid'</span>, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
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">749</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>;
|
|
|
|
granite.mass_top = <span class="org-highlight-numbers-number">4000</span>; <span class="org-comment">% [kg] TODO</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Dynamical Properties</span></span>
|
|
<span class="org-keyword">if</span> opts.rigid
|
|
granite.k.x = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% [N/m]</span>
|
|
granite.k.y = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% [N/m]</span>
|
|
granite.k.z = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% [N/m]</span>
|
|
granite.k.rx = <span class="org-highlight-numbers-number">1e10</span>; <span class="org-comment">% [N*m/deg]</span>
|
|
granite.k.ry = <span class="org-highlight-numbers-number">1e10</span>; <span class="org-comment">% [N*m/deg]</span>
|
|
granite.k.rz = <span class="org-highlight-numbers-number">1e10</span>; <span class="org-comment">% [N*m/deg]</span>
|
|
<span class="org-keyword">else</span>
|
|
granite.k.x = <span class="org-highlight-numbers-number">4e9</span>; <span class="org-comment">% [N/m]</span>
|
|
granite.k.y = <span class="org-highlight-numbers-number">3e8</span>; <span class="org-comment">% [N/m]</span>
|
|
granite.k.z = <span class="org-highlight-numbers-number">8e8</span>; <span class="org-comment">% [N/m]</span>
|
|
granite.k.rx = <span class="org-highlight-numbers-number">1e4</span>; <span class="org-comment">% [N*m/deg]</span>
|
|
granite.k.ry = <span class="org-highlight-numbers-number">1e4</span>; <span class="org-comment">% [N*m/deg]</span>
|
|
granite.k.rz = <span class="org-highlight-numbers-number">1e6</span>; <span class="org-comment">% [N*m/deg]</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
granite.c.x = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>granite.mass_top<span class="org-type">*</span>granite.k.x<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N/(m/s)]</span>
|
|
granite.c.y = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>granite.mass_top<span class="org-type">*</span>granite.k.y<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N/(m/s)]</span>
|
|
granite.c.z = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>granite.mass_top<span class="org-type">*</span>granite.k.z<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N/(m/s)]</span>
|
|
granite.c.rx = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>granite.mass_top<span class="org-type">*</span>granite.k.rx<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N*m/(deg/s)]</span>
|
|
granite.c.ry = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>granite.mass_top<span class="org-type">*</span>granite.k.ry<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N*m/(deg/s)]</span>
|
|
granite.c.rz = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>granite.mass_top<span class="org-type">*</span>granite.k.rz<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N*m/(deg/s)]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Positioning parameters</span></span>
|
|
granite.sample_pos = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">8</span>; <span class="org-comment">% Z-offset for the initial position of the sample [m]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'granite'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6e967d2" class="outline-3">
|
|
<h3 id="org6e967d2"><span class="section-number-3">6.6</span> Translation Stage</h3>
|
|
<div class="outline-text-3" id="text-6-6">
|
|
<p>
|
|
<a id="org2597c18"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeTy.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">ty</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeTy</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rigid'</span>, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
ty = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Y-Translation - Static Properties</span></span>
|
|
<span class="org-comment">% Ty Granite frame</span>
|
|
ty.granite_frame.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.granite_frame.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">753</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">753</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.granite_frame.STEP = <span class="org-string">'./STEPS/Ty/Ty_Granite_Frame.STEP'</span>;
|
|
<span class="org-comment">% Guide Translation Ty</span>
|
|
ty.guide.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.guide.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.guide.STEP = <span class="org-string">'./STEPS/ty/Ty_Guide.STEP'</span>;
|
|
<span class="org-comment">% Ty - Guide_Translation12</span>
|
|
ty.guide12.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.guide12.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.guide12.STEP = <span class="org-string">'./STEPS/Ty/Ty_Guide_12.STEP'</span>;
|
|
<span class="org-comment">% Ty - Guide_Translation11</span>
|
|
ty.guide11.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.guide11.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.guide11.STEP = <span class="org-string">'./STEPS/ty/Ty_Guide_11.STEP'</span>;
|
|
<span class="org-comment">% Ty - Guide_Translation22</span>
|
|
ty.guide22.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.guide22.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.guide22.STEP = <span class="org-string">'./STEPS/ty/Ty_Guide_22.STEP'</span>;
|
|
<span class="org-comment">% Ty - Guide_Translation21</span>
|
|
ty.guide21.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.guide21.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.guide21.STEP = <span class="org-string">'./STEPS/Ty/Ty_Guide_21.STEP'</span>;
|
|
<span class="org-comment">% Ty - Plateau translation</span>
|
|
ty.frame.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.frame.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.frame.STEP = <span class="org-string">'./STEPS/ty/Ty_Stage.STEP'</span>;
|
|
<span class="org-comment">% Ty Stator Part</span>
|
|
ty.stator.density = <span class="org-highlight-numbers-number">5400</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.stator.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.stator.STEP = <span class="org-string">'./STEPS/ty/Ty_Motor_Stator.STEP'</span>;
|
|
<span class="org-comment">% Ty Rotor Part</span>
|
|
ty.rotor.density = <span class="org-highlight-numbers-number">5400</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ty.rotor.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ty.rotor.STEP = <span class="org-string">'./STEPS/ty/Ty_Motor_Rotor.STEP'</span>;
|
|
|
|
ty.m = <span class="org-highlight-numbers-number">1000</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">1e12</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]
|
|
ty.k.rad = <span class="org-highlight-numbers-number">1e12</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]
|
|
<span class="org-keyword">else</span>
|
|
ty.k.ax = <span class="org-highlight-numbers-number">5e8</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]
|
|
ty.k.rad = <span class="org-highlight-numbers-number">5e7</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]
|
|
<span class="org-keyword">end</span>
|
|
|
|
ty.c.ax = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">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">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>ty.k.rad<span class="org-type">*</span>ty.m<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'ty'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6c95b53" class="outline-3">
|
|
<h3 id="org6c95b53"><span class="section-number-3">6.7</span> Tilt Stage</h3>
|
|
<div class="outline-text-3" id="text-6-7">
|
|
<p>
|
|
<a id="org29b1fab"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeRy.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">ry</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeRy</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rigid'</span>, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
ry = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Tilt Stage - Static Properties</span></span>
|
|
<span class="org-comment">% Ry - Guide for the tilt stage</span>
|
|
ry.guide.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ry.guide.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ry.guide.STEP = <span class="org-string">'./STEPS/ry/Tilt_Guide.STEP'</span>;
|
|
<span class="org-comment">% Ry - Rotor of the motor</span>
|
|
ry.rotor.density = <span class="org-highlight-numbers-number">2400</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ry.rotor.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ry.rotor.STEP = <span class="org-string">'./STEPS/ry/Tilt_Motor_Axis.STEP'</span>;
|
|
<span class="org-comment">% Ry - Motor</span>
|
|
ry.motor.density = <span class="org-highlight-numbers-number">3200</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ry.motor.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ry.motor.STEP = <span class="org-string">'./STEPS/ry/Tilt_Motor.STEP'</span>;
|
|
<span class="org-comment">% Ry - Plateau Tilt</span>
|
|
ry.stage.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
ry.stage.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
ry.stage.STEP = <span class="org-string">'./STEPS/ry/Tilt_Stage.STEP'</span>;
|
|
|
|
ry.m = <span class="org-highlight-numbers-number">800</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>
|
|
ry.k.h = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% Stiffness in the direction of the guidance [N/m]</span>
|
|
ry.k.rad = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% Stiffness in the top direction [N/m]</span>
|
|
ry.k.rrad = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% Stiffness in the side direction [N/m]</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>
|
|
ry.k.h = <span class="org-highlight-numbers-number">1e8</span>; <span class="org-comment">% Stiffness in the direction of the guidance [N/m]</span>
|
|
ry.k.rad = <span class="org-highlight-numbers-number">1e8</span>; <span class="org-comment">% Stiffness in the top direction [N/m]</span>
|
|
ry.k.rrad = <span class="org-highlight-numbers-number">1e8</span>; <span class="org-comment">% Stiffness in the side direction [N/m]</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
ry.c.h = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">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">0</span>.<span class="org-highlight-numbers-number">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">0</span>.<span class="org-highlight-numbers-number">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">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>ry.k.tilt<span class="org-type">*</span>ry.m<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Positioning parameters</span></span>
|
|
ry.z_offset = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">58178</span>; <span class="org-comment">% Z-Offset so that the center of rotation matches the sample center [m]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'ry'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org13271e7" class="outline-3">
|
|
<h3 id="org13271e7"><span class="section-number-3">6.8</span> Spindle</h3>
|
|
<div class="outline-text-3" id="text-6-8">
|
|
<p>
|
|
<a id="org50164cd"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeRz.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">rz</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeRz</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rigid'</span>, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
rz = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Spindle - Static Properties</span></span>
|
|
<span class="org-comment">% Spindle - Slip Ring</span>
|
|
rz.slipring.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
rz.slipring.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
rz.slipring.STEP = <span class="org-string">'./STEPS/rz/Spindle_Slip_Ring.STEP'</span>;
|
|
<span class="org-comment">% Spindle - Rotor</span>
|
|
rz.rotor.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
rz.rotor.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
rz.rotor.STEP = <span class="org-string">'./STEPS/rz/Spindle_Rotor.STEP'</span>;
|
|
<span class="org-comment">% Spindle - Stator</span>
|
|
rz.stator.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
rz.stator.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
rz.stator.STEP = <span class="org-string">'./STEPS/rz/Spindle_Stator.STEP'</span>;
|
|
|
|
<span class="org-comment">% Estimated mass of the mooving part</span>
|
|
rz.m = <span class="org-highlight-numbers-number">250</span>; <span class="org-comment">% [kg]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Spindle - Dynamical Properties</span></span>
|
|
|
|
<span class="org-keyword">if</span> opts.rigid
|
|
rz.k.rot = <span class="org-highlight-numbers-number">1e10</span>; <span class="org-comment">% Rotational Stiffness (Rz) [N*m/deg]</span>
|
|
rz.k.tilt = <span class="org-highlight-numbers-number">1e10</span>; <span class="org-comment">% Rotational Stiffness (Rx, Ry) [N*m/deg]</span>
|
|
rz.k.ax = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% Axial Stiffness (Z) [N/m]</span>
|
|
rz.k.rad = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% Radial Stiffness (X, Y) [N/m]</span>
|
|
<span class="org-keyword">else</span>
|
|
rz.k.rot = <span class="org-highlight-numbers-number">1e6</span>; <span class="org-comment">% TODO - Rotational Stiffness (Rz) [N*m/deg]</span>
|
|
rz.k.tilt = <span class="org-highlight-numbers-number">1e6</span>; <span class="org-comment">% Rotational Stiffness (Rx, Ry) [N*m/deg]</span>
|
|
rz.k.ax = <span class="org-highlight-numbers-number">2e9</span>; <span class="org-comment">% Axial Stiffness (Z) [N/m]</span>
|
|
rz.k.rad = <span class="org-highlight-numbers-number">7e8</span>; <span class="org-comment">% Radial Stiffness (X, Y) [N/m]</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-comment">% Damping</span>
|
|
rz.c.ax = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</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">0</span>.<span class="org-highlight-numbers-number">1</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">0</span>.<span class="org-highlight-numbers-number">1</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">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>rz.k.rot<span class="org-type">*</span>rz.m<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'rz'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3c482fe" class="outline-3">
|
|
<h3 id="org3c482fe"><span class="section-number-3">6.9</span> Initialize Hexapod legs' length</h3>
|
|
<div class="outline-text-3" id="text-6-9">
|
|
<p>
|
|
<a id="orgee2252f"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeHexapodPosition.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">hexapod</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeHexapodPosition</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">hexapod</span>, <span class="org-variable-name">AP</span>, <span class="org-variable-name">ARB</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-comment">% initializeHexapodPosition -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: initializeHexapodPosition(hexapod, AP, ARB)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - hexapod - Hexapod object containing the geometry of the hexapod</span>
|
|
<span class="org-comment">% - AP - Position vector of point OB expressed in frame {A}</span>
|
|
<span class="org-comment">% - ARB - Rotation Matrix expressed in frame {A}</span>
|
|
|
|
L0 = 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-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">L</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span>
|
|
Bbi = hexapod.pos_top_tranform<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>;
|
|
Aai = hexapod.pos_base<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>;
|
|
|
|
L0<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = sqrt<span class="org-rainbow-delimiters-depth-1">(</span>AP<span class="org-type">'*</span>AP <span class="org-type">+</span> Bbi<span class="org-type">'*</span>Bbi <span class="org-type">+</span> Aai<span class="org-type">'*</span>Aai <span class="org-type">-</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span>Aai <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<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><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>Aai<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
hexapod.L0 = L0;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7c295aa" class="outline-3">
|
|
<h3 id="org7c295aa"><span class="section-number-3">6.10</span> Micro Hexapod</h3>
|
|
<div class="outline-text-3" id="text-6-10">
|
|
<p>
|
|
<a id="org9e25f5d"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeMicroHexapod.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">micro_hexapod</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeMicroHexapod</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
|
<span class="org-string">'rigid'</span>, <span class="org-constant">false</span>, ...
|
|
<span class="org-string">'AP'</span>, zeros<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-comment"> % Wanted position in [m] of OB with respect to frame {A}</span>
|
|
<span class="org-string">'ARB'</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-comment"> % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}</span>
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Stewart Object</span></span>
|
|
micro_hexapod = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
micro_hexapod.h = <span class="org-highlight-numbers-number">350</span>; <span class="org-comment">% Total height of the platform [mm]</span>
|
|
micro_hexapod.jacobian = <span class="org-highlight-numbers-number">270</span>; <span class="org-comment">% Distance from the top of the mobile 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>
|
|
<span class="org-keyword">if</span> opts.rigid
|
|
Leg.k.ax = <span class="org-highlight-numbers-number">1e12</span>; <span class="org-comment">% Stiffness of each leg [N/m]</span>
|
|
<span class="org-keyword">else</span>
|
|
Leg.k.ax = <span class="org-highlight-numbers-number">2e7</span>; <span class="org-comment">% Stiffness of each leg [N/m]</span>
|
|
<span class="org-keyword">end</span>
|
|
Leg.ksi.ax = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span>; % Modal damping ksi = <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>c<span class="org-type">/</span>sqrt(km) []
|
|
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">%% Setup equilibrium position of each leg</span></span>
|
|
micro_hexapod.L0 = initializeHexapodPosition<span class="org-rainbow-delimiters-depth-1">(</span>micro_hexapod, opts.AP, opts.ARB<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'micro_hexapod'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">element</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">updateDamping</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">element</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
field = fieldnames<span class="org-rainbow-delimiters-depth-1">(</span>element.k<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
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">2</span><span class="org-type">*</span>element.ksi.<span class="org-rainbow-delimiters-depth-1">(</span>field<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>element.k.<span class="org-rainbow-delimiters-depth-2">(</span>field<span class="org-rainbow-delimiters-depth-3">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-3">}</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span>element.m<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">stewart</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeParameters</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">stewart</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Connection points on base and top plate w.r.t. World frame at the center of the base plate</span></span>
|
|
stewart.pos_base = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
stewart.pos_top = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
alpha_b = stewart.BP.leg.ang<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; % angle de décalage par rapport à <span class="org-highlight-numbers-number">120</span> deg (pour positionner les supports bases)
|
|
alpha_t = stewart.TP.leg.ang<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; % <span class="org-type">+-</span> offset angle from <span class="org-highlight-numbers-number">120</span> degree spacing on top
|
|
|
|
height = <span class="org-rainbow-delimiters-depth-1">(</span>stewart.h<span class="org-type">-</span>stewart.BP.thickness<span class="org-type">-</span>stewart.TP.thickness<span class="org-type">-</span>stewart.Leg.sphere.bottom<span class="org-type">-</span>stewart.Leg.sphere.top<span class="org-type">-</span>stewart.SP.thickness.bottom<span class="org-type">-</span>stewart.SP.thickness.top<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">001</span>; <span class="org-comment">% TODO</span>
|
|
|
|
radius_b = stewart.BP.leg.rad<span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">001</span>; <span class="org-comment">% rayon emplacement support base</span>
|
|
radius_t = stewart.TP.leg.rad<span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">001</span>; <span class="org-comment">% top radius in meters</span>
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
<span class="org-comment">% base points</span>
|
|
angle_m_b = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> alpha_b;
|
|
angle_p_b = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">+</span> alpha_b;
|
|
stewart.pos_base<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_b<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_b<span class="org-rainbow-delimiters-depth-2">)</span>, radius_b<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_b<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
stewart.pos_base<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_b<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_b<span class="org-rainbow-delimiters-depth-2">)</span>, radius_b<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_b<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-comment">% top points</span>
|
|
% Top points are <span class="org-highlight-numbers-number">60</span> degrees offset
|
|
angle_m_t = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> alpha_t <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">6</span>;
|
|
angle_p_t = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">+</span> alpha_t <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">6</span>;
|
|
stewart.pos_top<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_t<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_t<span class="org-rainbow-delimiters-depth-2">)</span>, radius_t<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_t<span class="org-rainbow-delimiters-depth-2">)</span>, height<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
stewart.pos_top<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_t<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_t<span class="org-rainbow-delimiters-depth-2">)</span>, radius_t<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_t<span class="org-rainbow-delimiters-depth-2">)</span>, height<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-comment">% permute pos_top points so that legs are end points of base and top points</span>
|
|
stewart.pos_top = <span class="org-rainbow-delimiters-depth-1">[</span>stewart.pos_top<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>; stewart.pos_top<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">5</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>; %<span class="org-highlight-numbers-number">6th</span> point on top connects to <span class="org-highlight-numbers-number">1st</span> on bottom
|
|
stewart.pos_top_tranform = stewart.pos_top <span class="org-type">-</span> height<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>,ones<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% leg vectors</span></span>
|
|
legs = stewart.pos_top <span class="org-type">-</span> stewart.pos_base;
|
|
leg_length = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
leg_vectors = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
leg_length<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = norm<span class="org-rainbow-delimiters-depth-1">(</span>legs<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
leg_vectors<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = legs<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">/</span> leg_length<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
stewart.Leg.lenght = <span class="org-highlight-numbers-number">1000</span><span class="org-type">*</span>leg_length<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">1</span>.<span class="org-highlight-numbers-number">5</span>;
|
|
stewart.Leg.shape.bot = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>; ...
|
|
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; ...
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>stewart.Leg.lenght<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Calculate revolute and cylindrical axes</span></span>
|
|
rev1 = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
rev2 = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
cyl1 = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
rev1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = cross<span class="org-rainbow-delimiters-depth-1">(</span>leg_vectors<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
rev1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = rev1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">/</span> norm<span class="org-rainbow-delimiters-depth-1">(</span>rev1<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
rev2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-type">-</span> cross<span class="org-rainbow-delimiters-depth-1">(</span>rev1<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, leg_vectors<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
rev2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = rev2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">/</span> norm<span class="org-rainbow-delimiters-depth-1">(</span>rev2<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
cyl1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = leg_vectors<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Coordinate systems</span></span>
|
|
stewart.lower_leg = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rotation'</span>, eye<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
stewart.upper_leg = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rotation'</span>, eye<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Position Matrix</span></span>
|
|
stewart.M_pos_base = stewart.pos_base <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-1">(</span>height<span class="org-type">+</span><span class="org-rainbow-delimiters-depth-2">(</span>stewart.TP.thickness<span class="org-type">+</span>stewart.Leg.sphere.top<span class="org-type">+</span>stewart.SP.thickness.top<span class="org-type">+</span>stewart.jacobian<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>,ones<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Compute Jacobian Matrix</span></span>
|
|
aa = stewart.pos_top_tranform <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-1">(</span>stewart.jacobian <span class="org-type">-</span> stewart.TP.thickness <span class="org-type">-</span> stewart.SP.height.top<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>,ones<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
stewart.J = getJacobianMatrix<span class="org-rainbow-delimiters-depth-1">(</span>leg_vectors<span class="org-type">'</span>, aa<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">%%</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name">J</span> = <span class="org-function-name">getJacobianMatrix</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">RM</span>, <span class="org-variable-name">M_pos_base</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
% RM<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] unit vector of each leg in the fixed frame
|
|
% M_pos_base<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] vector of the leg connection at the top platform location in the fixed frame
|
|
J = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
J<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = RM<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>;
|
|
<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">L</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeHexapodPosition</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">hexapod</span>, <span class="org-variable-name">AP</span>, <span class="org-variable-name">ARB</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-comment">% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: initializeHexapodPosition(hexapod, AP, ARB)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - hexapod - Hexapod object containing the geometry of the hexapod</span>
|
|
<span class="org-comment">% - AP - Position vector of point OB expressed in frame {A} in [m]</span>
|
|
<span class="org-comment">% - ARB - Rotation Matrix expressed in frame {A}</span>
|
|
|
|
<span class="org-comment">% Wanted Length of the hexapod's legs [m]</span>
|
|
L = 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-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">L</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span>
|
|
Bbi = hexapod.pos_top_tranform<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> <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><span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">0</span> ; hexapod.TP.thickness<span class="org-type">+</span>hexapod.Leg.sphere.top<span class="org-type">+</span>hexapod.SP.thickness.top<span class="org-type">+</span>hexapod.jacobian<span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
|
|
Aai = hexapod.pos_base<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> <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><span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">0</span> ; hexapod.BP.thickness<span class="org-type">+</span>hexapod.Leg.sphere.bottom<span class="org-type">+</span>hexapod.SP.thickness.bottom<span class="org-type">-</span>micro_hexapod.h<span class="org-type">-</span>hexapod.jacobian<span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
|
|
|
|
L<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = sqrt<span class="org-rainbow-delimiters-depth-1">(</span>AP<span class="org-type">'*</span>AP <span class="org-type">+</span> Bbi<span class="org-type">'*</span>Bbi <span class="org-type">+</span> Aai<span class="org-type">'*</span>Aai <span class="org-type">-</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span>Aai <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<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><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>Aai<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>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org72492f3" class="outline-3">
|
|
<h3 id="org72492f3"><span class="section-number-3">6.11</span> Center of gravity compensation</h3>
|
|
<div class="outline-text-3" id="text-6-11">
|
|
<p>
|
|
<a id="org991aec0"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeAxisc.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">axisc</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeAxisc</span><span class="org-rainbow-delimiters-depth-1">()</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
axisc = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Axis Compensator - Static Properties</span></span>
|
|
<span class="org-comment">% Structure</span>
|
|
axisc.structure.density = <span class="org-highlight-numbers-number">3400</span>; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.structure.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
axisc.structure.STEP = <span class="org-string">'./STEPS/axisc/axisc_structure.STEP'</span>;
|
|
<span class="org-comment">% Wheel</span>
|
|
axisc.wheel.density = <span class="org-highlight-numbers-number">2700</span>; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.wheel.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">753</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">753</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">753</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
axisc.wheel.STEP = <span class="org-string">'./STEPS/axisc/axisc_wheel.STEP'</span>;
|
|
<span class="org-comment">% Mass</span>
|
|
axisc.mass.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.mass.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
axisc.mass.STEP = <span class="org-string">'./STEPS/axisc/axisc_mass.STEP'</span>;
|
|
<span class="org-comment">% Gear</span>
|
|
axisc.gear.density = <span class="org-highlight-numbers-number">7800</span>; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.gear.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">792</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">820</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">933</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
axisc.gear.STEP = <span class="org-string">'./STEPS/axisc/axisc_gear.STEP'</span>;
|
|
|
|
axisc.m = <span class="org-highlight-numbers-number">40</span>; <span class="org-comment">% TODO [kg]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Axis Compensator - Dynamical Properties</span></span>
|
|
% axisc.k.ax = <span class="org-highlight-numbers-number">1</span>; % TODO [N<span class="org-type">*</span>m<span class="org-type">/</span>deg)]
|
|
% axisc.c.ax = (<span class="org-highlight-numbers-number">1</span><span class="org-type">/</span><span class="org-highlight-numbers-number">1</span>)<span class="org-type">*</span>sqrt(axisc.k.ax<span class="org-type">/</span>axisc.m);
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'axisc'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org68d031b" class="outline-3">
|
|
<h3 id="org68d031b"><span class="section-number-3">6.12</span> Mirror</h3>
|
|
<div class="outline-text-3" id="text-6-12">
|
|
<p>
|
|
<a id="org43cadec"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeMirror.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[]</span></span> = <span class="org-function-name">initializeMirror</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
|
<span class="org-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> ...
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
mirror = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
mirror.h = <span class="org-highlight-numbers-number">50</span>; <span class="org-comment">% height of the mirror [mm]</span>
|
|
mirror.thickness = <span class="org-highlight-numbers-number">25</span>; <span class="org-comment">% Thickness of the plate supporting the sample [mm]</span>
|
|
mirror.hole_rad = <span class="org-highlight-numbers-number">120</span>; <span class="org-comment">% radius of the hole in the mirror [mm]</span>
|
|
mirror.support_rad = <span class="org-highlight-numbers-number">100</span>; <span class="org-comment">% radius of the support plate [mm]</span>
|
|
mirror.jacobian = <span class="org-highlight-numbers-number">150</span>; <span class="org-comment">% point of interest offset in z (above the top surfave) [mm]</span>
|
|
mirror.rad = <span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% radius of the mirror (at the bottom surface) [mm]</span>
|
|
|
|
mirror.density = <span class="org-highlight-numbers-number">2400</span>; <span class="org-comment">% Density of the mirror [kg/m3]</span>
|
|
mirror.color = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">4</span> <span class="org-highlight-numbers-number">1</span>.<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span>.<span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% Color of the mirror</span>
|
|
|
|
mirror.cone_length = mirror.rad<span class="org-type">*</span>tand<span class="org-rainbow-delimiters-depth-1">(</span>opts.angle<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">+</span>mirror.h<span class="org-type">+</span>mirror.jacobian; <span class="org-comment">% Distance from Apex point of the cone to jacobian point</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Shape</span></span>
|
|
mirror.shape = <span class="org-rainbow-delimiters-depth-1">[</span>...
|
|
<span class="org-highlight-numbers-number">0</span> mirror.h<span class="org-type">-</span>mirror.thickness
|
|
mirror.hole_rad mirror.h<span class="org-type">-</span>mirror.thickness; ...
|
|
mirror.hole_rad <span class="org-highlight-numbers-number">0</span>; ...
|
|
mirror.rad <span class="org-highlight-numbers-number">0</span> ...
|
|
<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-keyword">if</span> strcmp<span class="org-rainbow-delimiters-depth-1">(</span>opts.shape, <span class="org-string">'spherical'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
mirror.sphere_radius = sqrt<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>mirror.jacobian<span class="org-type">+</span>mirror.h<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">+</span>mirror.rad<span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Radius of the sphere [mm]</span>
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">z</span> = <span class="org-constant">linspace</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant"><span class="org-highlight-numbers-number">0</span></span><span class="org-constant">, mirror.h, </span><span class="org-constant"><span class="org-highlight-numbers-number">101</span></span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span>
|
|
mirror.shape = <span class="org-rainbow-delimiters-depth-1">[</span>mirror.shape; sqrt<span class="org-rainbow-delimiters-depth-2">(</span>mirror.sphere_radius<span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">-</span><span class="org-rainbow-delimiters-depth-3">(</span>z<span class="org-type">-</span>mirror.jacobian<span class="org-type">-</span>mirror.h<span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span> z<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">elseif</span> strcmp<span class="org-rainbow-delimiters-depth-1">(</span>opts.shape, <span class="org-string">'conical'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
mirror.shape = <span class="org-rainbow-delimiters-depth-1">[</span>mirror.shape; mirror.rad<span class="org-type">+</span>mirror.h<span class="org-type">/</span>tand<span class="org-rainbow-delimiters-depth-2">(</span>opts.angle<span class="org-rainbow-delimiters-depth-2">)</span> mirror.h<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
<span class="org-keyword">else</span>
|
|
error<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Shape should be either conical or spherical'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
mirror.shape = <span class="org-rainbow-delimiters-depth-1">[</span>mirror.shape; <span class="org-highlight-numbers-number">0</span> mirror.h<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'mirror'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org74149ff" class="outline-3">
|
|
<h3 id="org74149ff"><span class="section-number-3">6.13</span> Nano Hexapod</h3>
|
|
<div class="outline-text-3" id="text-6-13">
|
|
<p>
|
|
<a id="orgcb27cac"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeNanoHexapod.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">nano_hexapod</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeNanoHexapod</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Stewart Object</span></span>
|
|
nano_hexapod = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
nano_hexapod.h = <span class="org-highlight-numbers-number">90</span>; <span class="org-comment">% Total height of the platform [mm]</span>
|
|
nano_hexapod.jacobian = <span class="org-highlight-numbers-number">175</span>; <span class="org-comment">% Point where the Jacobian is computed => Center of rotation [mm]</span>
|
|
% nano_hexapod.jacobian = <span class="org-highlight-numbers-number">174</span>.<span class="org-highlight-numbers-number">26</span>; % Point where the Jacobian is computed =<span class="org-type">></span> Center of rotation [mm]
|
|
|
|
<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">0</span>;
|
|
|
|
SP.thickness.bottom = SP.height.bottom<span class="org-type">-</span>Leg.sphere.bottom; <span class="org-comment">% [mm]</span>
|
|
SP.thickness.top = SP.height.top<span class="org-type">-</span>Leg.sphere.top; <span class="org-comment">% [mm]</span>
|
|
SP.rad.bottom = Leg.sphere.bottom; <span class="org-comment">% [mm]</span>
|
|
SP.rad.top = Leg.sphere.top; <span class="org-comment">% [mm]</span>
|
|
SP.m = SP.density.bottom<span class="org-type">*</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>SP.rad.bottom<span class="org-type">*</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span>SP.height.bottom<span class="org-type">*</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% TODO [kg]</span>
|
|
|
|
SP = updateDamping<span class="org-rainbow-delimiters-depth-1">(</span>SP<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
Leg.support.bottom = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> SP.thickness.bottom; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>; SP.rad.bottom <span class="org-highlight-numbers-number">0</span>; SP.rad.bottom SP.height.bottom<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
Leg.support.top = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> SP.thickness.top; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>; SP.rad.top <span class="org-highlight-numbers-number">0</span>; SP.rad.top SP.height.top<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
nano_hexapod.BP = BP;
|
|
nano_hexapod.TP = TP;
|
|
nano_hexapod.Leg = Leg;
|
|
nano_hexapod.SP = SP;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
nano_hexapod = initializeParameters<span class="org-rainbow-delimiters-depth-1">(</span>nano_hexapod<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'nano_hexapod'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">element</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">updateDamping</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">element</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
field = fieldnames<span class="org-rainbow-delimiters-depth-1">(</span>element.k<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
<span class="org-keyword">if</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> <span class="org-highlight-numbers-number">0</span>
|
|
element.c.<span class="org-rainbow-delimiters-depth-1">(</span>field<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">1</span><span class="org-type">/</span>element.ksi.<span class="org-rainbow-delimiters-depth-1">(</span>field<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>element.k.<span class="org-rainbow-delimiters-depth-2">(</span>field<span class="org-rainbow-delimiters-depth-3">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-3">}</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span>element.m<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">else</span>
|
|
element.c.<span class="org-rainbow-delimiters-depth-1">(</span>field<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">0</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">stewart</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeParameters</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">stewart</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Connection points on base and top plate w.r.t. World frame at the center of the base plate</span></span>
|
|
stewart.pos_base = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
stewart.pos_top = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
alpha_b = stewart.BP.leg.ang<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; % angle de décalage par rapport à <span class="org-highlight-numbers-number">120</span> deg (pour positionner les supports bases)
|
|
alpha_t = stewart.TP.leg.ang<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; % <span class="org-type">+-</span> offset angle from <span class="org-highlight-numbers-number">120</span> degree spacing on top
|
|
|
|
height = <span class="org-rainbow-delimiters-depth-1">(</span>stewart.h<span class="org-type">-</span>stewart.BP.thickness<span class="org-type">-</span>stewart.TP.thickness<span class="org-type">-</span>stewart.Leg.sphere.bottom<span class="org-type">-</span>stewart.Leg.sphere.top<span class="org-type">-</span>stewart.SP.thickness.bottom<span class="org-type">-</span>stewart.SP.thickness.top<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">001</span>; <span class="org-comment">% TODO</span>
|
|
|
|
radius_b = stewart.BP.leg.rad<span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">001</span>; <span class="org-comment">% rayon emplacement support base</span>
|
|
radius_t = stewart.TP.leg.rad<span class="org-type">*</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">001</span>; <span class="org-comment">% top radius in meters</span>
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
<span class="org-comment">% base points</span>
|
|
angle_m_b = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> alpha_b;
|
|
angle_p_b = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">+</span> alpha_b;
|
|
stewart.pos_base<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_b<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_b<span class="org-rainbow-delimiters-depth-2">)</span>, radius_b<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_b<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
stewart.pos_base<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_b<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_b<span class="org-rainbow-delimiters-depth-2">)</span>, radius_b<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_b<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-comment">% top points</span>
|
|
% Top points are <span class="org-highlight-numbers-number">60</span> degrees offset
|
|
angle_m_t = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> alpha_t <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">6</span>;
|
|
angle_p_t = <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">+</span> alpha_t <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">6</span>;
|
|
stewart.pos_top<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_t<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_t<span class="org-rainbow-delimiters-depth-2">)</span>, radius_t<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_m_t<span class="org-rainbow-delimiters-depth-2">)</span>, height<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
stewart.pos_top<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>radius_t<span class="org-type">*</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_t<span class="org-rainbow-delimiters-depth-2">)</span>, radius_t<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>angle_p_t<span class="org-rainbow-delimiters-depth-2">)</span>, height<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-comment">% permute pos_top points so that legs are end points of base and top points</span>
|
|
stewart.pos_top = <span class="org-rainbow-delimiters-depth-1">[</span>stewart.pos_top<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>; stewart.pos_top<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">5</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>; %<span class="org-highlight-numbers-number">6th</span> point on top connects to <span class="org-highlight-numbers-number">1st</span> on bottom
|
|
stewart.pos_top_tranform = stewart.pos_top <span class="org-type">-</span> height<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>,ones<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% leg vectors</span></span>
|
|
legs = stewart.pos_top <span class="org-type">-</span> stewart.pos_base;
|
|
leg_length = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
leg_vectors = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
leg_length<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = norm<span class="org-rainbow-delimiters-depth-1">(</span>legs<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
leg_vectors<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = legs<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">/</span> leg_length<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
stewart.Leg.lenght = <span class="org-highlight-numbers-number">1000</span><span class="org-type">*</span>leg_length<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">1</span>.<span class="org-highlight-numbers-number">5</span>;
|
|
stewart.Leg.shape.bot = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>; ...
|
|
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; ...
|
|
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>stewart.Leg.lenght<span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Calculate revolute and cylindrical axes</span></span>
|
|
rev1 = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
rev2 = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
cyl1 = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>
|
|
rev1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = cross<span class="org-rainbow-delimiters-depth-1">(</span>leg_vectors<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
rev1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = rev1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">/</span> norm<span class="org-rainbow-delimiters-depth-1">(</span>rev1<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
rev2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-type">-</span> cross<span class="org-rainbow-delimiters-depth-1">(</span>rev1<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, leg_vectors<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
rev2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = rev2<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">/</span> norm<span class="org-rainbow-delimiters-depth-1">(</span>rev2<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
cyl1<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = leg_vectors<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Coordinate systems</span></span>
|
|
stewart.lower_leg = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rotation'</span>, eye<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
stewart.upper_leg = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rotation'</span>, eye<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><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>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Position Matrix</span></span>
|
|
stewart.M_pos_base = stewart.pos_base <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-1">(</span>height<span class="org-type">+</span><span class="org-rainbow-delimiters-depth-2">(</span>stewart.TP.thickness<span class="org-type">+</span>stewart.Leg.sphere.top<span class="org-type">+</span>stewart.SP.thickness.top<span class="org-type">+</span>stewart.jacobian<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">*</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>,ones<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Compute Jacobian Matrix</span></span>
|
|
aa = stewart.pos_top_tranform <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-1">(</span>stewart.jacobian <span class="org-type">-</span> stewart.TP.thickness <span class="org-type">-</span> stewart.SP.height.top<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">*</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>,ones<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
|
stewart.J = getJacobianMatrix<span class="org-rainbow-delimiters-depth-1">(</span>leg_vectors<span class="org-type">'</span>, aa<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">function</span> <span class="org-variable-name">J</span> = <span class="org-function-name">getJacobianMatrix</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">RM</span>,<span class="org-variable-name">M_pos_base</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
% RM<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] unit vector of each leg in the fixed frame
|
|
% M_pos_base<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] vector of the leg connection at the top platform location in the fixed frame
|
|
J = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
J<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = RM<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>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org94d9a68" class="outline-3">
|
|
<h3 id="org94d9a68"><span class="section-number-3">6.14</span> Cedrat Actuator</h3>
|
|
<div class="outline-text-3" id="text-6-14">
|
|
<p>
|
|
<a id="orgdb4500f"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeCedratPiezo.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">cedrat</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeCedratPiezo</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
opts = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Stewart Object</span></span>
|
|
cedrat = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
|
|
cedrat.k = <span class="org-highlight-numbers-number">10e7</span>; <span class="org-comment">% Linear Stiffness of each "blade" [N/m]</span>
|
|
cedrat.ka = <span class="org-highlight-numbers-number">10e7</span>; <span class="org-comment">% Linear Stiffness of the stack [N/m]</span>
|
|
|
|
cedrat.c = <span class="org-highlight-numbers-number">0</span>.<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">1</span><span class="org-type">*</span>cedrat.k<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N/(m/s)]</span>
|
|
cedrat.ca = <span class="org-highlight-numbers-number">0</span>.<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">1</span><span class="org-type">*</span>cedrat.ka<span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [N/(m/s)]</span>
|
|
|
|
cedrat.L = <span class="org-highlight-numbers-number">80</span>; <span class="org-comment">% Total Width of the Actuator[mm]</span>
|
|
cedrat.H = <span class="org-highlight-numbers-number">45</span>; <span class="org-comment">% Total Height of the Actuator [mm]</span>
|
|
cedrat.L2 = sqrt<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>cedrat.L<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-highlight-numbers-number">2</span> <span class="org-type">+</span> <span class="org-rainbow-delimiters-depth-2">(</span>cedrat.H<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-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Length of the elipsoidal sections [mm]</span>
|
|
cedrat.alpha = <span class="org-highlight-numbers-number">180</span><span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>atan2<span class="org-rainbow-delimiters-depth-1">(</span>cedrat.L<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>, cedrat.H<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">% [deg]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'cedrat'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8ceaacc" class="outline-3">
|
|
<h3 id="org8ceaacc"><span class="section-number-3">6.15</span> Sample</h3>
|
|
<div class="outline-text-3" id="text-6-15">
|
|
<p>
|
|
<a id="org96164b7"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="../src/initializeSample.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">sample</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeSample</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
|
|
sample = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'radius'</span>, <span class="org-highlight-numbers-number">100</span>, ...
|
|
<span class="org-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> ...
|
|
<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
|
|
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span>
|
|
sample.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
sample.k.x = <span class="org-highlight-numbers-number">1e8</span>;
|
|
sample.c.x = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>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>;
|
|
|
|
sample.k.y = <span class="org-highlight-numbers-number">1e8</span>;
|
|
sample.c.y = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>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>;
|
|
|
|
sample.k.z = <span class="org-highlight-numbers-number">1e8</span>;
|
|
sample.c.z = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span><span class="org-type">*</span>sqrt<span class="org-rainbow-delimiters-depth-1">(</span>sample.k.z<span class="org-type">*</span>sample.mass<span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'sample'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2019-12-10 mar. 18:05</p>
|
|
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
|
|
</div>
|
|
</body>
|
|
</html>
|