2971 lines
147 KiB
HTML
2971 lines
147 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>
|
|
<!-- 2021-02-20 sam. 23:08 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<title>Subsystems used for the Simscape Models</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
|
|
<script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></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">Subsystems used for the Simscape Models</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#orgb7baa4c">1. Simscape Configuration</a>
|
|
<ul>
|
|
<li><a href="#orgbf25db9">Function description</a></li>
|
|
<li><a href="#org9536b74">Optional Parameters</a></li>
|
|
<li><a href="#orge9bb660">Structure initialization</a></li>
|
|
<li><a href="#orgad1af12">Add Type</a></li>
|
|
<li><a href="#orgfa89c5c">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org7e1df5d">2. Logging Configuration</a>
|
|
<ul>
|
|
<li><a href="#org3ae2ef2">Function description</a></li>
|
|
<li><a href="#org4c4a2f7">Optional Parameters</a></li>
|
|
<li><a href="#orgfbcac63">Structure initialization</a></li>
|
|
<li><a href="#org23ec5d5">Add Type</a></li>
|
|
<li><a href="#org9355a23">Sampling Time</a></li>
|
|
<li><a href="#orgcd8a7f6">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgceb1db0">3. Ground</a>
|
|
<ul>
|
|
<li><a href="#orgc9016cd">Simscape Model</a></li>
|
|
<li><a href="#org1a59ddf">Function description</a></li>
|
|
<li><a href="#org021917a">Optional Parameters</a></li>
|
|
<li><a href="#org00d7f77">Structure initialization</a></li>
|
|
<li><a href="#orgcac4016">Add Type</a></li>
|
|
<li><a href="#org7598026">Ground Solid properties</a></li>
|
|
<li><a href="#org24663d9">Rotation Point</a></li>
|
|
<li><a href="#org18e3e42">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org5e616f7">4. Granite</a>
|
|
<ul>
|
|
<li><a href="#orgfdf0447">Simscape Model</a></li>
|
|
<li><a href="#org340caf8">Function description</a></li>
|
|
<li><a href="#orgd94a86f">Optional Parameters</a></li>
|
|
<li><a href="#org22c2f9e">Structure initialization</a></li>
|
|
<li><a href="#org4c5a016">Add Granite Type</a></li>
|
|
<li><a href="#org71c4c4c">Material and Geometry</a></li>
|
|
<li><a href="#orgfbcb7bc">Stiffness and Damping properties</a></li>
|
|
<li><a href="#org8713d49">Equilibrium position of the each joint.</a></li>
|
|
<li><a href="#orgb9e0c59">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org36ada7d">5. Translation Stage</a>
|
|
<ul>
|
|
<li><a href="#orgb631240">Simscape Model</a></li>
|
|
<li><a href="#org82074cf">Function description</a></li>
|
|
<li><a href="#orgb79f831">Optional Parameters</a></li>
|
|
<li><a href="#orgc0d606c">Structure initialization</a></li>
|
|
<li><a href="#org88330b8">Add Translation Stage Type</a></li>
|
|
<li><a href="#org3b678f6">Material and Geometry</a></li>
|
|
<li><a href="#orgfd47d92">Stiffness and Damping properties</a></li>
|
|
<li><a href="#org4397215">Equilibrium position of the each joint.</a></li>
|
|
<li><a href="#org45adbbd">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orga747e36">6. Tilt Stage</a>
|
|
<ul>
|
|
<li><a href="#orgf974a8f">Simscape Model</a></li>
|
|
<li><a href="#orgf8a0706">Function description</a></li>
|
|
<li><a href="#org53efc16">Optional Parameters</a></li>
|
|
<li><a href="#orgb9fe877">Structure initialization</a></li>
|
|
<li><a href="#org2ce78bb">Add Tilt Type</a></li>
|
|
<li><a href="#orga8ac3bf">Material and Geometry</a></li>
|
|
<li><a href="#org854e39b">Stiffness and Damping properties</a></li>
|
|
<li><a href="#org2e8c08d">Equilibrium position of the each joint.</a></li>
|
|
<li><a href="#orge8845c9">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orga315869">7. Spindle</a>
|
|
<ul>
|
|
<li><a href="#org68ca295">Simscape Model</a></li>
|
|
<li><a href="#org0f16bac">Function description</a></li>
|
|
<li><a href="#org851ebc8">Optional Parameters</a></li>
|
|
<li><a href="#orgfeb3dd5">Structure initialization</a></li>
|
|
<li><a href="#org9898239">Add Spindle Type</a></li>
|
|
<li><a href="#orgda99a0f">Material and Geometry</a></li>
|
|
<li><a href="#orgca03f34">Stiffness and Damping properties</a></li>
|
|
<li><a href="#org5cc06db">Equilibrium position of the each joint.</a></li>
|
|
<li><a href="#org413df37">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org43fbd9e">8. Micro Hexapod</a>
|
|
<ul>
|
|
<li><a href="#orgda01cab">Simscape Model</a></li>
|
|
<li><a href="#orga9b6c29">Function description</a></li>
|
|
<li><a href="#org6667ae2">Optional Parameters</a></li>
|
|
<li><a href="#orgf3f4170">Function content</a></li>
|
|
<li><a href="#orga052a18">Add Type</a></li>
|
|
<li><a href="#org71f5e83">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgc9e4f54">9. Center of gravity compensation</a>
|
|
<ul>
|
|
<li><a href="#org7269fd2">Simscape Model</a></li>
|
|
<li><a href="#org26577d5">Function description</a></li>
|
|
<li><a href="#org744f209">Optional Parameters</a></li>
|
|
<li><a href="#org211a6c8">Structure initialization</a></li>
|
|
<li><a href="#org91710d0">Add Type</a></li>
|
|
<li><a href="#org62775b6">Material and Geometry</a></li>
|
|
<li><a href="#orgf17eb34">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgd192273">10. Mirror</a>
|
|
<ul>
|
|
<li><a href="#orgc56d79a">Simscape Model</a></li>
|
|
<li><a href="#org1c464d8">Function description</a></li>
|
|
<li><a href="#orgb29fe27">Optional Parameters</a></li>
|
|
<li><a href="#orgc8d7e37">Structure initialization</a></li>
|
|
<li><a href="#org8d13d05">Add Mirror Type</a></li>
|
|
<li><a href="#orgc8e217b">Mass and Inertia</a></li>
|
|
<li><a href="#org12185f0">Stiffness and Damping properties</a></li>
|
|
<li><a href="#org7bfc10d">Equilibrium position of the each joint.</a></li>
|
|
<li><a href="#org69f6320">Geometry</a></li>
|
|
<li><a href="#org4ef9e2c">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgbc684a6">11. Nano Hexapod</a>
|
|
<ul>
|
|
<li><a href="#org3105a36">Simscape Model</a></li>
|
|
<li><a href="#orga240ae4">Function description</a></li>
|
|
<li><a href="#org6e8b704">Optional Parameters</a></li>
|
|
<li><a href="#org2bae7e6">Function content</a></li>
|
|
<li><a href="#orgc446fb4">Add Type</a></li>
|
|
<li><a href="#orgc4b3b65">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgbc058d5">12. Sample</a>
|
|
<ul>
|
|
<li><a href="#org83ab8d1">Simscape Model</a></li>
|
|
<li><a href="#org97e8fa7">Function description</a></li>
|
|
<li><a href="#orgee69310">Optional Parameters</a></li>
|
|
<li><a href="#org5c6a4c0">Structure initialization</a></li>
|
|
<li><a href="#org7211784">Add Sample Type</a></li>
|
|
<li><a href="#orgb205d51">Material and Geometry</a></li>
|
|
<li><a href="#orgc8a58fa">Compute the Inertia</a></li>
|
|
<li><a href="#org112cacd">Stiffness and Damping properties</a></li>
|
|
<li><a href="#org27401d0">Equilibrium position of the each joint.</a></li>
|
|
<li><a href="#org8c3f548">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org08f6708">13. Initialize Controller</a>
|
|
<ul>
|
|
<li><a href="#orgcd721e8">Function Declaration and Documentation</a></li>
|
|
<li><a href="#orgb1de776">Optional Parameters</a></li>
|
|
<li><a href="#orgc6bb2bb">Structure initialization</a></li>
|
|
<li><a href="#orgf0db064">Controller Type</a></li>
|
|
<li><a href="#org443e57e">Save the Structure</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgc5f188e">14. Generate Reference Signals</a>
|
|
<ul>
|
|
<li><a href="#orgdef7115">Function Declaration and Documentation</a></li>
|
|
<li><a href="#orga1c48e4">Optional Parameters</a></li>
|
|
<li><a href="#orgb2488be">Initialize Parameters</a></li>
|
|
<li><a href="#orgc0941f6">Translation Stage</a></li>
|
|
<li><a href="#org148b614">Tilt Stage</a></li>
|
|
<li><a href="#org591aa90">Spindle</a></li>
|
|
<li><a href="#org5fca80a">Micro Hexapod</a></li>
|
|
<li><a href="#orgc8390ff">Axis Compensation</a></li>
|
|
<li><a href="#org5ddf967">Nano Hexapod</a></li>
|
|
<li><a href="#orgac26a7c">Save</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org8106d91">15. Initialize Disturbances</a>
|
|
<ul>
|
|
<li><a href="#orge05ed22">Function Declaration and Documentation</a></li>
|
|
<li><a href="#orgbb2f4ad">Optional Parameters</a></li>
|
|
<li><a href="#orgb224bf9">Load Data</a></li>
|
|
<li><a href="#org4c390fe">Parameters</a></li>
|
|
<li><a href="#org2ea1ed6">Ground Motion</a></li>
|
|
<li><a href="#orgb784e1f">Translation Stage - X direction</a></li>
|
|
<li><a href="#orgf93582d">Translation Stage - Z direction</a></li>
|
|
<li><a href="#orgcf7bc46">Spindle - Z direction</a></li>
|
|
<li><a href="#org105c318">Direct Forces</a></li>
|
|
<li><a href="#orgb4d9177">Set initial value to zero</a></li>
|
|
<li><a href="#orgaf8c9f7">Save</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orge4aa590">16. Initialize Position Errors</a>
|
|
<ul>
|
|
<li><a href="#org3633d18">Function Declaration and Documentation</a></li>
|
|
<li><a href="#org37567d0">Optional Parameters</a></li>
|
|
<li><a href="#org51e7ef5">Structure initialization</a></li>
|
|
<li><a href="#org7e10ff7">Type</a></li>
|
|
<li><a href="#org3fa3c9c">Position Errors</a></li>
|
|
<li><a href="#orgca6a79f">Save</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org10272ac">17. Z-Axis Geophone</a></li>
|
|
<li><a href="#orga3cba43">18. Z-Axis Accelerometer</a></li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
The full Simscape Model is represented in Figure <a href="#org28f26c5">1</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org28f26c5" class="figure">
|
|
<p><img src="figs/images/simscape_picture.png" alt="simscape_picture.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 1: </span>Screenshot of the Multi-Body Model representation</p>
|
|
</div>
|
|
|
|
<p>
|
|
This model is divided into multiple subsystems that are independent.
|
|
These subsystems are saved in separate files and imported in the main file using a block balled “subsystem reference”.
|
|
</p>
|
|
|
|
<p>
|
|
Each stage is configured (geometry, mass properties, dynamic properties …) using one function.
|
|
</p>
|
|
|
|
<p>
|
|
These functions are defined below.
|
|
</p>
|
|
|
|
<div id="outline-container-orgb7baa4c" class="outline-2">
|
|
<h2 id="orgb7baa4c"><span class="section-number-2">1</span> Simscape Configuration</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org47be044"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbf25db9" class="outline-3">
|
|
<h3 id="orgbf25db9">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgbf25db9">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">initializeSimscapeConfiguration</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9536b74" class="outline-3">
|
|
<h3 id="org9536b74">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org9536b74">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.gravity logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge9bb660" class="outline-3">
|
|
<h3 id="orge9bb660">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-orge9bb660">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> conf_simscape = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgad1af12" class="outline-3">
|
|
<h3 id="orgad1af12">Add Type</h3>
|
|
<div class="outline-text-3" id="text-orgad1af12">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.gravity
|
|
conf_simscape.type = 1;
|
|
<span class="org-keyword">else</span>
|
|
conf_simscape.type = 2;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfa89c5c" class="outline-3">
|
|
<h3 id="orgfa89c5c">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-orgfa89c5c">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/conf_simscape.mat'</span>, <span class="org-string">'conf_simscape'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7e1df5d" class="outline-2">
|
|
<h2 id="org7e1df5d"><span class="section-number-2">2</span> Logging Configuration</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="org199d255"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org3ae2ef2" class="outline-3">
|
|
<h3 id="org3ae2ef2">Function description</h3>
|
|
<div class="outline-text-3" id="text-org3ae2ef2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">initializeLoggingConfiguration</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4c4a2f7" class="outline-3">
|
|
<h3 id="org4c4a2f7">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org4c4a2f7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.log char {mustBeMember(args.log,{<span class="org-string">'none'</span>, <span class="org-string">'all'</span>, <span class="org-string">'forces'</span>})} = <span class="org-string">'none'</span>
|
|
<span class="org-variable-name">args</span>.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e<span class="org-type">-</span>3
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfbcac63" class="outline-3">
|
|
<h3 id="orgfbcac63">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-orgfbcac63">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> conf_log = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org23ec5d5" class="outline-3">
|
|
<h3 id="org23ec5d5">Add Type</h3>
|
|
<div class="outline-text-3" id="text-org23ec5d5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.log</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
conf_log.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'all'</span>
|
|
conf_log.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'forces'</span>
|
|
conf_log.type = 2;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9355a23" class="outline-3">
|
|
<h3 id="org9355a23">Sampling Time</h3>
|
|
<div class="outline-text-3" id="text-org9355a23">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> conf_log.Ts = args.Ts;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcd8a7f6" class="outline-3">
|
|
<h3 id="orgcd8a7f6">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-orgcd8a7f6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/conf_log.mat'</span>, <span class="org-string">'conf_log'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgceb1db0" class="outline-2">
|
|
<h2 id="orgceb1db0"><span class="section-number-2">3</span> Ground</h2>
|
|
<div class="outline-text-2" id="text-3">
|
|
<p>
|
|
<a id="orgcce2443"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc9016cd" class="outline-3">
|
|
<h3 id="orgc9016cd">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-orgc9016cd">
|
|
<p>
|
|
The model of the Ground is composed of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>A <b>Cartesian</b> joint that is used to simulation the ground motion</li>
|
|
<li>A solid that represents the ground on which the granite is located</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgaf3e336" class="figure">
|
|
<p><img src="figs/images/simscape_model_ground.png" alt="simscape_model_ground.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 2: </span>Simscape model for the Ground</p>
|
|
</div>
|
|
|
|
|
|
<div id="org85b6c35" class="figure">
|
|
<p><img src="figs/images/simscape_picture_ground.png" alt="simscape_picture_ground.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 3: </span>Simscape picture for the Ground</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1a59ddf" class="outline-3">
|
|
<h3 id="org1a59ddf">Function description</h3>
|
|
<div class="outline-text-3" id="text-org1a59ddf">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[ground]</span> = <span class="org-function-name">initializeGround</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org021917a" class="outline-3">
|
|
<h3 id="org021917a">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org021917a">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>})} = <span class="org-string">'rigid'</span>
|
|
<span class="org-variable-name">args</span>.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) <span class="org-comment">% Rotation point for the ground motion [m]</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org00d7f77" class="outline-3">
|
|
<h3 id="org00d7f77">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-org00d7f77">
|
|
<p>
|
|
First, we initialize the <code>granite</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ground = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcac4016" class="outline-3">
|
|
<h3 id="orgcac4016">Add Type</h3>
|
|
<div class="outline-text-3" id="text-orgcac4016">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
ground.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
ground.type = 1;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7598026" class="outline-3">
|
|
<h3 id="org7598026">Ground Solid properties</h3>
|
|
<div class="outline-text-3" id="text-org7598026">
|
|
<p>
|
|
We set the shape and density of the ground solid element.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ground.shape = [2, 2, 0.5]; <span class="org-comment">% [m]</span>
|
|
ground.density = 2800; <span class="org-comment">% [kg/m3]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org24663d9" class="outline-3">
|
|
<h3 id="org24663d9">Rotation Point</h3>
|
|
<div class="outline-text-3" id="text-org24663d9">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ground.rot_point = args.rot_point;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org18e3e42" class="outline-3">
|
|
<h3 id="org18e3e42">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-org18e3e42">
|
|
<p>
|
|
The <code>ground</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'ground'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5e616f7" class="outline-2">
|
|
<h2 id="org5e616f7"><span class="section-number-2">4</span> Granite</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
<p>
|
|
<a id="orgd1d6d6e"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfdf0447" class="outline-3">
|
|
<h3 id="orgfdf0447">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-orgfdf0447">
|
|
<p>
|
|
The Simscape model of the granite is composed of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>A cartesian joint such that the granite can vibrations along x, y and z axis</li>
|
|
<li>A solid</li>
|
|
</ul>
|
|
|
|
<p>
|
|
The output <code>sample_pos</code> corresponds to the impact point of the X-ray.
|
|
</p>
|
|
|
|
|
|
<div id="org6d4d402" class="figure">
|
|
<p><img src="figs/images/simscape_model_granite.png" alt="simscape_model_granite.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 4: </span>Simscape model for the Granite</p>
|
|
</div>
|
|
|
|
|
|
<div id="org3ba1752" class="figure">
|
|
<p><img src="figs/images/simscape_picture_granite.png" alt="simscape_picture_granite.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 5: </span>Simscape picture for the Granite</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org340caf8" class="outline-3">
|
|
<h3 id="org340caf8">Function description</h3>
|
|
<div class="outline-text-3" id="text-org340caf8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[granite]</span> = <span class="org-function-name">initializeGranite</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd94a86f" class="outline-3">
|
|
<h3 id="orgd94a86f">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgd94a86f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>, <span class="org-string">'none'</span>, <span class="org-string">'modal-analysis'</span>, <span class="org-string">'init'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-variable-name">args</span>.Foffset logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-variable-name">args</span>.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 <span class="org-comment">% Density [kg/m3]</span>
|
|
<span class="org-variable-name">args</span>.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] <span class="org-comment">% [N/m]</span>
|
|
<span class="org-variable-name">args</span>.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] <span class="org-comment">% [N/(m/s)]</span>
|
|
<span class="org-variable-name">args</span>.x0 (1,1) double {mustBeNumeric} = 0 <span class="org-comment">% Rest position of the Joint in the X direction [m]</span>
|
|
<span class="org-variable-name">args</span>.y0 (1,1) double {mustBeNumeric} = 0 <span class="org-comment">% Rest position of the Joint in the Y direction [m]</span>
|
|
<span class="org-variable-name">args</span>.z0 (1,1) double {mustBeNumeric} = 0 <span class="org-comment">% Rest position of the Joint in the Z direction [m]</span>
|
|
<span class="org-variable-name">args</span>.sample_pos (1,1) double {mustBeNumeric} = 0.8 <span class="org-comment">% Height of the measurment point [m]</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org22c2f9e" class="outline-3">
|
|
<h3 id="org22c2f9e">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-org22c2f9e">
|
|
<p>
|
|
First, we initialize the <code>granite</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> granite = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4c5a016" class="outline-3">
|
|
<h3 id="org4c5a016">Add Granite Type</h3>
|
|
<div class="outline-text-3" id="text-org4c5a016">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
granite.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
granite.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
granite.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'modal-analysis'</span>
|
|
granite.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'init'</span>
|
|
granite.type = 4;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org71c4c4c" class="outline-3">
|
|
<h3 id="org71c4c4c">Material and Geometry</h3>
|
|
<div class="outline-text-3" id="text-org71c4c4c">
|
|
<p>
|
|
Properties of the Material and link to the geometry of the granite.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> granite.density = args.density; <span class="org-comment">% [kg/m3]</span>
|
|
granite.STEP = <span class="org-string">'./STEPS/granite/granite.STEP'</span>;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Z-offset for the initial position of the sample with respect to the granite top surface.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> granite.sample_pos = args.sample_pos; <span class="org-comment">% [m]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfbcb7bc" class="outline-3">
|
|
<h3 id="orgfbcb7bc">Stiffness and Damping properties</h3>
|
|
<div class="outline-text-3" id="text-orgfbcb7bc">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> granite.K = args.K; <span class="org-comment">% [N/m]</span>
|
|
granite.C = args.C; <span class="org-comment">% [N/(m/s)]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8713d49" class="outline-3">
|
|
<h3 id="org8713d49">Equilibrium position of the each joint.</h3>
|
|
<div class="outline-text-3" id="text-org8713d49">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Foffset <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'none'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'rigid'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'init'</span>)
|
|
load(<span class="org-string">'mat/Foffset.mat'</span>, <span class="org-string">'Fgm'</span>);
|
|
granite.Deq = <span class="org-type">-</span>Fgm<span class="org-type">'./</span>granite.K;
|
|
<span class="org-keyword">else</span>
|
|
granite.Deq = zeros(6,1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb9e0c59" class="outline-3">
|
|
<h3 id="orgb9e0c59">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-orgb9e0c59">
|
|
<p>
|
|
The <code>granite</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'granite'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org36ada7d" class="outline-2">
|
|
<h2 id="org36ada7d"><span class="section-number-2">5</span> Translation Stage</h2>
|
|
<div class="outline-text-2" id="text-5">
|
|
<p>
|
|
<a id="orgf864345"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb631240" class="outline-3">
|
|
<h3 id="orgb631240">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-orgb631240">
|
|
<p>
|
|
The Simscape model of the Translation stage consist of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>One rigid body for the fixed part of the translation stage</li>
|
|
<li>One rigid body for the moving part of the translation stage</li>
|
|
<li>Four 6-DOF Joints that only have some rigidity in the X and Z directions.
|
|
The rigidity in rotation comes from the fact that we use multiple joints that are located at different points</li>
|
|
<li>One 6-DOF joint that represent the Actuator.
|
|
It is used to impose the motion in the Y direction</li>
|
|
<li>One 6-DOF joint to inject force disturbance in the X and Z directions</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgae125fb" class="figure">
|
|
<p><img src="figs/images/simscape_model_ty.png" alt="simscape_model_ty.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 6: </span>Simscape model for the Translation Stage</p>
|
|
</div>
|
|
|
|
|
|
<div id="org6312fa6" class="figure">
|
|
<p><img src="figs/images/simscape_picture_ty.png" alt="simscape_picture_ty.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 7: </span>Simscape picture for the Translation Stage</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org82074cf" class="outline-3">
|
|
<h3 id="org82074cf">Function description</h3>
|
|
<div class="outline-text-3" id="text-org82074cf">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[ty]</span> = <span class="org-function-name">initializeTy</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb79f831" class="outline-3">
|
|
<h3 id="orgb79f831">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgb79f831">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>, <span class="org-string">'modal-analysis'</span>, <span class="org-string">'init'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-variable-name">args</span>.Foffset logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc0d606c" class="outline-3">
|
|
<h3 id="orgc0d606c">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-orgc0d606c">
|
|
<p>
|
|
First, we initialize the <code>ty</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ty = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org88330b8" class="outline-3">
|
|
<h3 id="org88330b8">Add Translation Stage Type</h3>
|
|
<div class="outline-text-3" id="text-org88330b8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
ty.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
ty.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
ty.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'modal-analysis'</span>
|
|
ty.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'init'</span>
|
|
ty.type = 4;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3b678f6" class="outline-3">
|
|
<h3 id="org3b678f6">Material and Geometry</h3>
|
|
<div class="outline-text-3" id="text-org3b678f6">
|
|
<p>
|
|
Define the density of the materials as well as the geometry (STEP files).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-comment">% Ty Granite frame</span>
|
|
ty.granite_frame.density = 7800; <span class="org-comment">% [kg/m3] => 43kg</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 = 7800; <span class="org-comment">% [kg/m3] => 76kg</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 = 7800; <span class="org-comment">% [kg/m3]</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 = 7800; <span class="org-comment">% [kg/m3]</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 = 7800; <span class="org-comment">% [kg/m3]</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 = 7800; <span class="org-comment">% [kg/m3]</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 = 7800; <span class="org-comment">% [kg/m3]</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 = 5400; <span class="org-comment">% [kg/m3]</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 = 5400; <span class="org-comment">% [kg/m3]</span>
|
|
ty.rotor.STEP = <span class="org-string">'./STEPS/ty/Ty_Motor_Rotor.STEP'</span>;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfd47d92" class="outline-3">
|
|
<h3 id="orgfd47d92">Stiffness and Damping properties</h3>
|
|
<div class="outline-text-3" id="text-orgfd47d92">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; <span class="org-comment">% [N/m, N*m/rad]</span>
|
|
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; <span class="org-comment">% [N/(m/s), N*m/(rad/s)]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4397215" class="outline-3">
|
|
<h3 id="org4397215">Equilibrium position of the each joint.</h3>
|
|
<div class="outline-text-3" id="text-org4397215">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Foffset <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'none'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'rigid'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'init'</span>)
|
|
load(<span class="org-string">'mat/Foffset.mat'</span>, <span class="org-string">'Ftym'</span>);
|
|
ty.Deq = <span class="org-type">-</span>Ftym<span class="org-type">'./</span>ty.K;
|
|
<span class="org-keyword">else</span>
|
|
ty.Deq = zeros(6,1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org45adbbd" class="outline-3">
|
|
<h3 id="org45adbbd">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-org45adbbd">
|
|
<p>
|
|
The <code>ty</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'ty'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga747e36" class="outline-2">
|
|
<h2 id="orga747e36"><span class="section-number-2">6</span> Tilt Stage</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="org56c9ce4"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf974a8f" class="outline-3">
|
|
<h3 id="orgf974a8f">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-orgf974a8f">
|
|
<p>
|
|
The Simscape model of the Tilt stage is composed of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Two solid bodies for the two part of the stage</li>
|
|
<li><b>Four</b> 6-DOF joints to model the flexibility of the stage.
|
|
These joints are virtually located along the rotation axis and are connecting the two solid bodies.
|
|
These joints have some translation stiffness in the u-v-w directions aligned with the joint.
|
|
The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations</li>
|
|
<li>A Bushing Joint used for the Actuator.
|
|
The Ry motion is imposed by the input.</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orged083f7" class="figure">
|
|
<p><img src="figs/images/simscape_model_ry.png" alt="simscape_model_ry.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 8: </span>Simscape model for the Tilt Stage</p>
|
|
</div>
|
|
|
|
|
|
<div id="org6e1dd5d" class="figure">
|
|
<p><img src="figs/images/simscape_picture_ry.png" alt="simscape_picture_ry.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 9: </span>Simscape picture for the Tilt Stage</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf8a0706" class="outline-3">
|
|
<h3 id="orgf8a0706">Function description</h3>
|
|
<div class="outline-text-3" id="text-orgf8a0706">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[ry]</span> = <span class="org-function-name">initializeRy</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org53efc16" class="outline-3">
|
|
<h3 id="org53efc16">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org53efc16">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>, <span class="org-string">'modal-analysis'</span>, <span class="org-string">'init'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-variable-name">args</span>.Foffset logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-variable-name">args</span>.Ry_init (1,1) double {mustBeNumeric} = 0
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb9fe877" class="outline-3">
|
|
<h3 id="orgb9fe877">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-orgb9fe877">
|
|
<p>
|
|
First, we initialize the <code>ry</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ry = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org2ce78bb" class="outline-3">
|
|
<h3 id="org2ce78bb">Add Tilt Type</h3>
|
|
<div class="outline-text-3" id="text-org2ce78bb">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
ry.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
ry.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
ry.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'modal-analysis'</span>
|
|
ry.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'init'</span>
|
|
ry.type = 4;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga8ac3bf" class="outline-3">
|
|
<h3 id="orga8ac3bf">Material and Geometry</h3>
|
|
<div class="outline-text-3" id="text-orga8ac3bf">
|
|
<p>
|
|
Properties of the Material and link to the geometry of the Tilt stage.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-comment">% Ry - Guide for the tilt stage</span>
|
|
ry.guide.density = 7800; <span class="org-comment">% [kg/m3]</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 = 2400; <span class="org-comment">% [kg/m3]</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 = 3200; <span class="org-comment">% [kg/m3]</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 = 7800; <span class="org-comment">% [kg/m3]</span>
|
|
ry.stage.STEP = <span class="org-string">'./STEPS/ry/Tilt_Stage.STEP'</span>;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Z-Offset so that the center of rotation matches the sample center;
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ry.z_offset = 0.58178; <span class="org-comment">% [m]</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ry.Ry_init = args.Ry_init; <span class="org-comment">% [rad]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org854e39b" class="outline-3">
|
|
<h3 id="org854e39b">Stiffness and Damping properties</h3>
|
|
<div class="outline-text-3" id="text-org854e39b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
|
|
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2e8c08d" class="outline-3">
|
|
<h3 id="org2e8c08d">Equilibrium position of the each joint.</h3>
|
|
<div class="outline-text-3" id="text-org2e8c08d">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Foffset <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'none'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'rigid'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'init'</span>)
|
|
load(<span class="org-string">'mat/Foffset.mat'</span>, <span class="org-string">'Fym'</span>);
|
|
ry.Deq = <span class="org-type">-</span>Fym<span class="org-type">'./</span>ry.K;
|
|
<span class="org-keyword">else</span>
|
|
ry.Deq = zeros(6,1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge8845c9" class="outline-3">
|
|
<h3 id="orge8845c9">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-orge8845c9">
|
|
<p>
|
|
The <code>ry</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'ry'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga315869" class="outline-2">
|
|
<h2 id="orga315869"><span class="section-number-2">7</span> Spindle</h2>
|
|
<div class="outline-text-2" id="text-7">
|
|
<p>
|
|
<a id="orgbc5ca33"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org68ca295" class="outline-3">
|
|
<h3 id="org68ca295">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-org68ca295">
|
|
<p>
|
|
The Simscape model of the Spindle is composed of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Two rigid bodies: the stator and the rotor</li>
|
|
<li>A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction.</li>
|
|
<li>The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgb3e44bb" class="figure">
|
|
<p><img src="figs/images/simscape_model_rz.png" alt="simscape_model_rz.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 10: </span>Simscape model for the Spindle</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgfd36775" class="figure">
|
|
<p><img src="figs/images/simscape_picture_rz.png" alt="simscape_picture_rz.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 11: </span>Simscape picture for the Spindle</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0f16bac" class="outline-3">
|
|
<h3 id="org0f16bac">Function description</h3>
|
|
<div class="outline-text-3" id="text-org0f16bac">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[rz]</span> = <span class="org-function-name">initializeRz</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org851ebc8" class="outline-3">
|
|
<h3 id="org851ebc8">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org851ebc8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>, <span class="org-string">'modal-analysis'</span>, <span class="org-string">'init'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-variable-name">args</span>.Foffset logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfeb3dd5" class="outline-3">
|
|
<h3 id="orgfeb3dd5">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-orgfeb3dd5">
|
|
<p>
|
|
First, we initialize the <code>rz</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> rz = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9898239" class="outline-3">
|
|
<h3 id="org9898239">Add Spindle Type</h3>
|
|
<div class="outline-text-3" id="text-org9898239">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
rz.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
rz.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
rz.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'modal-analysis'</span>
|
|
rz.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'init'</span>
|
|
rz.type = 4;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgda99a0f" class="outline-3">
|
|
<h3 id="orgda99a0f">Material and Geometry</h3>
|
|
<div class="outline-text-3" id="text-orgda99a0f">
|
|
<p>
|
|
Properties of the Material and link to the geometry of the spindle.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-comment">% Spindle - Slip Ring</span>
|
|
rz.slipring.density = 7800; <span class="org-comment">% [kg/m3]</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 = 7800; <span class="org-comment">% [kg/m3]</span>
|
|
rz.rotor.STEP = <span class="org-string">'./STEPS/rz/Spindle_Rotor.STEP'</span>;
|
|
|
|
<span class="org-comment">% Spindle - Stator</span>
|
|
rz.stator.density = 7800; <span class="org-comment">% [kg/m3]</span>
|
|
rz.stator.STEP = <span class="org-string">'./STEPS/rz/Spindle_Stator.STEP'</span>;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgca03f34" class="outline-3">
|
|
<h3 id="orgca03f34">Stiffness and Damping properties</h3>
|
|
<div class="outline-text-3" id="text-orgca03f34">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
|
|
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5cc06db" class="outline-3">
|
|
<h3 id="org5cc06db">Equilibrium position of the each joint.</h3>
|
|
<div class="outline-text-3" id="text-org5cc06db">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Foffset <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'none'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'rigid'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'init'</span>)
|
|
load(<span class="org-string">'mat/Foffset.mat'</span>, <span class="org-string">'Fzm'</span>);
|
|
rz.Deq = <span class="org-type">-</span>Fzm<span class="org-type">'./</span>rz.K;
|
|
<span class="org-keyword">else</span>
|
|
rz.Deq = zeros(6,1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org413df37" class="outline-3">
|
|
<h3 id="org413df37">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-org413df37">
|
|
<p>
|
|
The <code>rz</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'rz'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org43fbd9e" class="outline-2">
|
|
<h2 id="org43fbd9e"><span class="section-number-2">8</span> Micro Hexapod</h2>
|
|
<div class="outline-text-2" id="text-8">
|
|
<p>
|
|
<a id="org626bb9c"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgda01cab" class="outline-3">
|
|
<h3 id="orgda01cab">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-orgda01cab">
|
|
|
|
<div id="orgdba23ec" class="figure">
|
|
<p><img src="figs/images/simscape_model_micro_hexapod.png" alt="simscape_model_micro_hexapod.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 12: </span>Simscape model for the Micro-Hexapod</p>
|
|
</div>
|
|
|
|
|
|
<div id="org976465f" class="figure">
|
|
<p><img src="figs/images/simscape_picture_micro_hexapod.png" alt="simscape_picture_micro_hexapod.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 13: </span>Simscape picture for the Micro-Hexapod</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga9b6c29" class="outline-3">
|
|
<h3 id="orga9b6c29">Function description</h3>
|
|
<div class="outline-text-3" id="text-orga9b6c29">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[micro_hexapod]</span> = <span class="org-function-name">initializeMicroHexapod</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6667ae2" class="outline-3">
|
|
<h3 id="org6667ae2">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org6667ae2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>, <span class="org-string">'modal-analysis'</span>, <span class="org-string">'init'</span>, <span class="org-string">'compliance'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-comment">% initializeFramesPositions</span>
|
|
<span class="org-variable-name">args</span>.H (1,1) double {mustBeNumeric, mustBePositive} = 350e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.MO_B (1,1) double {mustBeNumeric} = 270e<span class="org-type">-</span>3
|
|
<span class="org-comment">% generateGeneralConfiguration</span>
|
|
<span class="org-variable-name">args</span>.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.FTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>10, 10, 120<span class="org-type">-</span>10, 120<span class="org-type">+</span>10, 240<span class="org-type">-</span>10, 240<span class="org-type">+</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180)
|
|
<span class="org-variable-name">args</span>.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.MTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>60<span class="org-type">+</span>10, 60<span class="org-type">-</span>10, 60<span class="org-type">+</span>10, 180<span class="org-type">-</span>10, 180<span class="org-type">+</span>10, <span class="org-type">-</span>60<span class="org-type">-</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180)
|
|
<span class="org-comment">% initializeStrutDynamics</span>
|
|
<span class="org-variable-name">args</span>.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-comment">% initializeCylindricalPlatforms</span>
|
|
<span class="org-variable-name">args</span>.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
|
<span class="org-variable-name">args</span>.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
|
<span class="org-variable-name">args</span>.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e<span class="org-type">-</span>3
|
|
<span class="org-comment">% initializeCylindricalStruts</span>
|
|
<span class="org-variable-name">args</span>.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-variable-name">args</span>.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-variable-name">args</span>.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e<span class="org-type">-</span>3
|
|
<span class="org-comment">% inverseKinematics</span>
|
|
<span class="org-variable-name">args</span>.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
<span class="org-variable-name">args</span>.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
<span class="org-comment">% Force that stiffness of each joint should apply at t=0</span>
|
|
<span class="org-variable-name">args</span>.Foffset logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf3f4170" class="outline-3">
|
|
<h3 id="orgf3f4170">Function content</h3>
|
|
<div class="outline-text-3" id="text-orgf3f4170">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeStewartPlatform();
|
|
|
|
stewart = initializeFramesPositions(stewart, ...
|
|
<span class="org-string">'H'</span>, args.H, ...
|
|
<span class="org-string">'MO_B'</span>, args.MO_B);
|
|
|
|
stewart = generateGeneralConfiguration(stewart, ...
|
|
<span class="org-string">'FH'</span>, args.FH, ...
|
|
<span class="org-string">'FR'</span>, args.FR, ...
|
|
<span class="org-string">'FTh'</span>, args.FTh, ...
|
|
<span class="org-string">'MH'</span>, args.MH, ...
|
|
<span class="org-string">'MR'</span>, args.MR, ...
|
|
<span class="org-string">'MTh'</span>, args.MTh);
|
|
|
|
stewart = computeJointsPose(stewart);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeStrutDynamics(stewart, ...
|
|
<span class="org-string">'K'</span>, args.Ki, ...
|
|
<span class="org-string">'C'</span>, args.Ci);
|
|
|
|
stewart = initializeJointDynamics(stewart, ...
|
|
<span class="org-string">'type_F'</span>, <span class="org-string">'universal_p'</span>, ...
|
|
<span class="org-string">'type_M'</span>, <span class="org-string">'spherical_p'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeCylindricalPlatforms(stewart, ...
|
|
<span class="org-string">'Fpm'</span>, args.Fpm, ...
|
|
<span class="org-string">'Fph'</span>, args.Fph, ...
|
|
<span class="org-string">'Fpr'</span>, args.Fpr, ...
|
|
<span class="org-string">'Mpm'</span>, args.Mpm, ...
|
|
<span class="org-string">'Mph'</span>, args.Mph, ...
|
|
<span class="org-string">'Mpr'</span>, args.Mpr);
|
|
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
<span class="org-string">'Fsm'</span>, args.Fsm, ...
|
|
<span class="org-string">'Fsh'</span>, args.Fsh, ...
|
|
<span class="org-string">'Fsr'</span>, args.Fsr, ...
|
|
<span class="org-string">'Msm'</span>, args.Msm, ...
|
|
<span class="org-string">'Msh'</span>, args.Msh, ...
|
|
<span class="org-string">'Msr'</span>, args.Msr);
|
|
|
|
stewart = computeJacobian(stewart);
|
|
|
|
stewart = initializeStewartPose(stewart, ...
|
|
<span class="org-string">'AP'</span>, args.AP, ...
|
|
<span class="org-string">'ARB'</span>, args.ARB);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Equilibrium position of the each joint.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Foffset <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'none'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'rigid'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'init'</span>)
|
|
load(<span class="org-string">'mat/Foffset.mat'</span>, <span class="org-string">'Fhm'</span>);
|
|
stewart.actuators.dLeq = <span class="org-type">-</span>Fhm<span class="org-type">'./</span>args.Ki;
|
|
<span class="org-keyword">else</span>
|
|
stewart.actuators.dLeq = zeros(6,1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga052a18" class="outline-3">
|
|
<h3 id="orga052a18">Add Type</h3>
|
|
<div class="outline-text-3" id="text-orga052a18">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
stewart.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
stewart.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
stewart.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'modal-analysis'</span>
|
|
stewart.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'init'</span>
|
|
stewart.type = 4;
|
|
<span class="org-keyword">case</span> <span class="org-string">'compliance'</span>
|
|
stewart.type = 5;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org71f5e83" class="outline-3">
|
|
<h3 id="org71f5e83">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-org71f5e83">
|
|
<p>
|
|
The <code>micro_hexapod</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> micro_hexapod = stewart;
|
|
save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'micro_hexapod'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc9e4f54" class="outline-2">
|
|
<h2 id="orgc9e4f54"><span class="section-number-2">9</span> Center of gravity compensation</h2>
|
|
<div class="outline-text-2" id="text-9">
|
|
<p>
|
|
<a id="orgbddace1"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org7269fd2" class="outline-3">
|
|
<h3 id="org7269fd2">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-org7269fd2">
|
|
<p>
|
|
The Simscape model of the Center of gravity compensator is composed of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>One main solid that is connected to two other solids (the masses to position of center of mass) through two revolute joints</li>
|
|
<li>The angle of both revolute joints is set by the input</li>
|
|
</ul>
|
|
|
|
|
|
<div id="org6939980" class="figure">
|
|
<p><img src="figs/images/simscape_model_axisc.png" alt="simscape_model_axisc.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 14: </span>Simscape model for the Center of Mass compensation system</p>
|
|
</div>
|
|
|
|
|
|
<div id="org843e065" class="figure">
|
|
<p><img src="figs/images/simscape_picture_axisc.png" alt="simscape_picture_axisc.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 15: </span>Simscape picture for the Center of Mass compensation system</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org26577d5" class="outline-3">
|
|
<h3 id="org26577d5">Function description</h3>
|
|
<div class="outline-text-3" id="text-org26577d5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[axisc]</span> = <span class="org-function-name">initializeAxisc</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org744f209" class="outline-3">
|
|
<h3 id="org744f209">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org744f209">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org211a6c8" class="outline-3">
|
|
<h3 id="org211a6c8">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-org211a6c8">
|
|
<p>
|
|
First, we initialize the <code>axisc</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> axisc = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org91710d0" class="outline-3">
|
|
<h3 id="org91710d0">Add Type</h3>
|
|
<div class="outline-text-3" id="text-org91710d0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
axisc.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
axisc.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
axisc.type = 2;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org62775b6" class="outline-3">
|
|
<h3 id="org62775b6">Material and Geometry</h3>
|
|
<div class="outline-text-3" id="text-org62775b6">
|
|
<p>
|
|
Properties of the Material and link to the geometry files.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-comment">% Structure</span>
|
|
axisc.structure.density = 3400; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.structure.STEP = <span class="org-string">'./STEPS/axisc/axisc_structure.STEP'</span>;
|
|
|
|
<span class="org-comment">% Wheel</span>
|
|
axisc.wheel.density = 2700; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.wheel.STEP = <span class="org-string">'./STEPS/axisc/axisc_wheel.STEP'</span>;
|
|
|
|
<span class="org-comment">% Mass</span>
|
|
axisc.mass.density = 7800; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.mass.STEP = <span class="org-string">'./STEPS/axisc/axisc_mass.STEP'</span>;
|
|
|
|
<span class="org-comment">% Gear</span>
|
|
axisc.gear.density = 7800; <span class="org-comment">% [kg/m3]</span>
|
|
axisc.gear.STEP = <span class="org-string">'./STEPS/axisc/axisc_gear.STEP'</span>;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf17eb34" class="outline-3">
|
|
<h3 id="orgf17eb34">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-orgf17eb34">
|
|
<p>
|
|
The <code>axisc</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'axisc'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd192273" class="outline-2">
|
|
<h2 id="orgd192273"><span class="section-number-2">10</span> Mirror</h2>
|
|
<div class="outline-text-2" id="text-10">
|
|
<p>
|
|
<a id="org99fbef6"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc56d79a" class="outline-3">
|
|
<h3 id="orgc56d79a">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-orgc56d79a">
|
|
<p>
|
|
The Simscape Model of the mirror is just a solid body.
|
|
The output <code>mirror_center</code> corresponds to the center of the Sphere and is the point of measurement for the metrology
|
|
</p>
|
|
|
|
|
|
<div id="orgcf9758c" class="figure">
|
|
<p><img src="figs/images/simscape_model_mirror.png" alt="simscape_model_mirror.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 16: </span>Simscape model for the Mirror</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgae41f15" class="figure">
|
|
<p><img src="figs/images/simscape_picture_mirror.png" alt="simscape_picture_mirror.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 17: </span>Simscape picture for the Mirror</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1c464d8" class="outline-3">
|
|
<h3 id="org1c464d8">Function description</h3>
|
|
<div class="outline-text-3" id="text-org1c464d8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">initializeMirror</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb29fe27" class="outline-3">
|
|
<h3 id="orgb29fe27">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgb29fe27">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>})} = <span class="org-string">'rigid'</span>
|
|
<span class="org-variable-name">args</span>.shape char {mustBeMember(args.shape,{<span class="org-string">'spherical'</span>, <span class="org-string">'conical'</span>})} = <span class="org-string">'spherical'</span>
|
|
<span class="org-variable-name">args</span>.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 <span class="org-comment">% [deg]</span>
|
|
<span class="org-variable-name">args</span>.mass (1,1) double {mustBeNumeric, mustBePositive} = 10 <span class="org-comment">% [kg]</span>
|
|
<span class="org-variable-name">args</span>.freq (6,1) double {mustBeNumeric, mustBeNonnegative} = 200<span class="org-type">*</span>ones(6,1) <span class="org-comment">% [Hz]</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc8d7e37" class="outline-3">
|
|
<h3 id="orgc8d7e37">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-orgc8d7e37">
|
|
<p>
|
|
First, we initialize the <code>mirror</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8d13d05" class="outline-3">
|
|
<h3 id="org8d13d05">Add Mirror Type</h3>
|
|
<div class="outline-text-3" id="text-org8d13d05">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
mirror.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
mirror.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
mirror.type = 2;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc8e217b" class="outline-3">
|
|
<h3 id="orgc8e217b">Mass and Inertia</h3>
|
|
<div class="outline-text-3" id="text-orgc8e217b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror.mass = args.mass;
|
|
mirror.freq = args.freq;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org12185f0" class="outline-3">
|
|
<h3 id="org12185f0">Stiffness and Damping properties</h3>
|
|
<div class="outline-text-3" id="text-org12185f0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror.K = zeros(6,1);
|
|
mirror.K(1<span class="org-type">:</span>3) = mirror.mass <span class="org-type">*</span> (2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>mirror.freq(1<span class="org-type">:</span>3))<span class="org-type">.^</span>2;
|
|
|
|
mirror.C = zeros(6,1);
|
|
mirror.C(1<span class="org-type">:</span>3) = 0.2 <span class="org-type">*</span> sqrt(mirror.K(1<span class="org-type">:</span>3)<span class="org-type">.*</span>mirror.mass);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7bfc10d" class="outline-3">
|
|
<h3 id="org7bfc10d">Equilibrium position of the each joint.</h3>
|
|
<div class="outline-text-3" id="text-org7bfc10d">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror.Deq = zeros(6,1);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org69f6320" class="outline-3">
|
|
<h3 id="org69f6320">Geometry</h3>
|
|
<div class="outline-text-3" id="text-org69f6320">
|
|
<p>
|
|
We define the geometrical values.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror.h = 0.05; <span class="org-comment">% Height of the mirror [m]</span>
|
|
|
|
mirror.thickness = 0.02; <span class="org-comment">% Thickness of the plate supporting the sample [m]</span>
|
|
|
|
mirror.hole_rad = 0.125; <span class="org-comment">% radius of the hole in the mirror [m]</span>
|
|
|
|
mirror.support_rad = 0.1; <span class="org-comment">% radius of the support plate [m]</span>
|
|
|
|
<span class="org-comment">% point of interest offset in z (above the top surfave) [m]</span>
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
mirror.jacobian = 0.205;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
mirror.jacobian = 0.205 <span class="org-type">-</span> mirror.h;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
mirror.jacobian = 0.205 <span class="org-type">-</span> mirror.h;
|
|
<span class="org-keyword">end</span>
|
|
|
|
mirror.rad = 0.180; <span class="org-comment">% radius of the mirror (at the bottom surface) [m]</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror.cone_length = mirror.rad<span class="org-type">*</span>tand(args.angle)<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>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Now we define the Shape of the mirror.
|
|
We first start with the internal part.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror.shape = [...
|
|
mirror.support_rad<span class="org-type">+</span>5e<span class="org-type">-</span>3 mirror.h<span class="org-type">-</span>mirror.thickness
|
|
mirror.hole_rad mirror.h<span class="org-type">-</span>mirror.thickness; ...
|
|
mirror.hole_rad 0; ...
|
|
mirror.rad 0 ...
|
|
];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Then, we define the reflective used part of the mirror.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> strcmp(args.shape, <span class="org-string">'spherical'</span>)
|
|
mirror.sphere_radius = sqrt((mirror.jacobian<span class="org-type">+</span>mirror.h)<span class="org-type">^</span>2<span class="org-type">+</span>mirror.rad<span class="org-type">^</span>2); <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(0, mirror.h, 101)</span>
|
|
mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius<span class="org-type">^</span>2<span class="org-type">-</span>(z<span class="org-type">-</span>mirror.jacobian<span class="org-type">-</span>mirror.h)<span class="org-type">^</span>2) z];
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">elseif</span> strcmp(args.shape, <span class="org-string">'conical'</span>)
|
|
mirror.shape = [mirror.shape; mirror.rad<span class="org-type">+</span>mirror.h<span class="org-type">/</span>tand(args.angle) mirror.h];
|
|
<span class="org-keyword">else</span>
|
|
error(<span class="org-string">'Shape should be either conical or spherical'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Finally, we close the shape.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> mirror.shape = [mirror.shape; mirror.support_rad<span class="org-type">+</span>5e<span class="org-type">-</span>3 mirror.h];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4ef9e2c" class="outline-3">
|
|
<h3 id="org4ef9e2c">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-org4ef9e2c">
|
|
<p>
|
|
The <code>mirror</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'mirror'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbc684a6" class="outline-2">
|
|
<h2 id="orgbc684a6"><span class="section-number-2">11</span> Nano Hexapod</h2>
|
|
<div class="outline-text-2" id="text-11">
|
|
<p>
|
|
<a id="org854fc3a"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org3105a36" class="outline-3">
|
|
<h3 id="org3105a36">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-org3105a36">
|
|
|
|
<div id="orgca97ee2" class="figure">
|
|
<p><img src="figs/images/simscape_model_nano_hexapod.png" alt="simscape_model_nano_hexapod.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 18: </span>Simscape model for the Nano Hexapod</p>
|
|
</div>
|
|
|
|
|
|
<div id="org71a31a6" class="figure">
|
|
<p><img src="figs/images/simscape_picture_nano_hexapod.png" alt="simscape_picture_nano_hexapod.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 19: </span>Simscape picture for the Nano Hexapod</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga240ae4" class="outline-3">
|
|
<h3 id="orga240ae4">Function description</h3>
|
|
<div class="outline-text-3" id="text-orga240ae4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[nano_hexapod]</span> = <span class="org-function-name">initializeNanoHexapod</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6e8b704" class="outline-3">
|
|
<h3 id="org6e8b704">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org6e8b704">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'none'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>, <span class="org-string">'init'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-comment">% initializeFramesPositions</span>
|
|
<span class="org-variable-name">args</span>.H (1,1) double {mustBeNumeric, mustBePositive} = 95e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.MO_B (1,1) double {mustBeNumeric} = 170e<span class="org-type">-</span>3
|
|
<span class="org-comment">% generateGeneralConfiguration</span>
|
|
<span class="org-variable-name">args</span>.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.FTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>10, 10, 120<span class="org-type">-</span>10, 120<span class="org-type">+</span>10, 240<span class="org-type">-</span>10, 240<span class="org-type">+</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180)
|
|
<span class="org-variable-name">args</span>.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.MTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>60<span class="org-type">+</span>10, 60<span class="org-type">-</span>10, 60<span class="org-type">+</span>10, 180<span class="org-type">-</span>10, 180<span class="org-type">+</span>10, <span class="org-type">-</span>60<span class="org-type">-</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180)
|
|
<span class="org-comment">% initializeStrutDynamics</span>
|
|
<span class="org-variable-name">args</span>.actuator char {mustBeMember(args.actuator,{<span class="org-string">'piezo'</span>, <span class="org-string">'lorentz'</span>, <span class="org-string">'amplified'</span>})} = <span class="org-string">'piezo'</span>
|
|
<span class="org-variable-name">args</span>.k1 (1,1) double {mustBeNumeric} = 1e6
|
|
<span class="org-variable-name">args</span>.ke (1,1) double {mustBeNumeric} = 5e6
|
|
<span class="org-variable-name">args</span>.ka (1,1) double {mustBeNumeric} = 60e6
|
|
<span class="org-variable-name">args</span>.c1 (1,1) double {mustBeNumeric} = 10
|
|
<span class="org-variable-name">args</span>.F_gain (1,1) double {mustBeNumeric} = 1
|
|
<span class="org-variable-name">args</span>.k (1,1) double {mustBeNumeric} = <span class="org-type">-</span>1
|
|
<span class="org-variable-name">args</span>.c (1,1) double {mustBeNumeric} = <span class="org-type">-</span>1
|
|
<span class="org-comment">% initializeJointDynamics</span>
|
|
<span class="org-variable-name">args</span>.type_F char {mustBeMember(args.type_F,{<span class="org-string">'universal'</span>, <span class="org-string">'spherical'</span>, <span class="org-string">'universal_p'</span>, <span class="org-string">'spherical_p'</span>, <span class="org-string">'universal_3dof'</span>, <span class="org-string">'spherical_3dof'</span>, <span class="org-string">'flexible'</span>})} = <span class="org-string">'universal'</span>
|
|
<span class="org-variable-name">args</span>.type_M char {mustBeMember(args.type_M,{<span class="org-string">'universal'</span>, <span class="org-string">'spherical'</span>, <span class="org-string">'universal_p'</span>, <span class="org-string">'spherical_p'</span>, <span class="org-string">'universal_3dof'</span>, <span class="org-string">'spherical_3dof'</span>, <span class="org-string">'flexible'</span>})} = <span class="org-string">'spherical'</span>
|
|
<span class="org-variable-name">args</span>.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>4<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>3<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>4<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e<span class="org-type">-</span>3<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-variable-name">args</span>.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1<span class="org-type">*</span>ones(6,1)
|
|
<span class="org-comment">% initializeCylindricalPlatforms</span>
|
|
<span class="org-variable-name">args</span>.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-variable-name">args</span>.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-variable-name">args</span>.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 120e<span class="org-type">-</span>3
|
|
<span class="org-comment">% initializeCylindricalStruts</span>
|
|
<span class="org-variable-name">args</span>.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
<span class="org-variable-name">args</span>.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
<span class="org-variable-name">args</span>.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e<span class="org-type">-</span>3
|
|
<span class="org-variable-name">args</span>.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e<span class="org-type">-</span>3
|
|
<span class="org-comment">% inverseKinematics</span>
|
|
<span class="org-variable-name">args</span>.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
<span class="org-variable-name">args</span>.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
<span class="org-comment">% Equilibrium position of each leg</span>
|
|
<span class="org-variable-name">args</span>.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
<span class="org-comment">% Force that stiffness of each joint should apply at t=0</span>
|
|
<span class="org-variable-name">args</span>.Foffset logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2bae7e6" class="outline-3">
|
|
<h3 id="org2bae7e6">Function content</h3>
|
|
<div class="outline-text-3" id="text-org2bae7e6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeStewartPlatform();
|
|
|
|
stewart = initializeFramesPositions(stewart, <span class="org-string">'H'</span>, args.H, <span class="org-string">'MO_B'</span>, args.MO_B);
|
|
|
|
stewart = generateGeneralConfiguration(stewart, <span class="org-string">'FH'</span>, args.FH, <span class="org-string">'FR'</span>, args.FR, <span class="org-string">'FTh'</span>, args.FTh, <span class="org-string">'MH'</span>, args.MH, <span class="org-string">'MR'</span>, args.MR, <span class="org-string">'MTh'</span>, args.MTh);
|
|
|
|
stewart = computeJointsPose(stewart);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.k <span class="org-type">></span> 0 <span class="org-type">&&</span> args.c <span class="org-type">></span> 0
|
|
stewart = initializeStrutDynamics(stewart, <span class="org-string">'type'</span>, <span class="org-string">'classical'</span>, <span class="org-string">'K'</span>, args.k<span class="org-type">*</span>ones(6,1), <span class="org-string">'C'</span>, args.c<span class="org-type">*</span>ones(6,1));
|
|
<span class="org-keyword">elseif</span> args.k <span class="org-type">></span> 0
|
|
stewart = initializeStrutDynamics(stewart, <span class="org-string">'type'</span>, <span class="org-string">'classical'</span>, <span class="org-string">'K'</span>, args.k<span class="org-type">*</span>ones(6,1), <span class="org-string">'C'</span>, 1.5<span class="org-type">*</span>sqrt(args.k)<span class="org-type">*</span>ones(6,1));
|
|
<span class="org-keyword">elseif</span> strcmp(args.actuator, <span class="org-string">'piezo'</span>)
|
|
stewart = initializeStrutDynamics(stewart, <span class="org-string">'type'</span>, <span class="org-string">'classical'</span>, <span class="org-string">'K'</span>, 1e7<span class="org-type">*</span>ones(6,1), <span class="org-string">'C'</span>, 1e2<span class="org-type">*</span>ones(6,1));
|
|
<span class="org-keyword">elseif</span> strcmp(args.actuator, <span class="org-string">'lorentz'</span>)
|
|
stewart = initializeStrutDynamics(stewart, <span class="org-string">'type'</span>, <span class="org-string">'classical'</span>, <span class="org-string">'K'</span>, 1e4<span class="org-type">*</span>ones(6,1), <span class="org-string">'C'</span>, 1e2<span class="org-type">*</span>ones(6,1));
|
|
<span class="org-keyword">elseif</span> strcmp(args.actuator, <span class="org-string">'amplified'</span>)
|
|
stewart = initializeStrutDynamics(stewart, <span class="org-string">'type'</span>, <span class="org-string">'amplified'</span>, ...
|
|
<span class="org-string">'k1'</span>, args.k1<span class="org-type">*</span>ones(6,1), ...
|
|
<span class="org-string">'c1'</span>, args.c1<span class="org-type">*</span>ones(6,1), ...
|
|
<span class="org-string">'ka'</span>, args.ka<span class="org-type">*</span>ones(6,1), ...
|
|
<span class="org-string">'ke'</span>, args.ke<span class="org-type">*</span>ones(6,1), ...
|
|
<span class="org-string">'F_gain'</span>, args.F_gain<span class="org-type">*</span>ones(6,1));
|
|
<span class="org-keyword">else</span>
|
|
error(<span class="org-string">'args.actuator should be piezo, lorentz or amplified'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeJointDynamics(stewart, ...
|
|
<span class="org-string">'type_F'</span>, args.type_F, ...
|
|
<span class="org-string">'type_M'</span>, args.type_M, ...
|
|
<span class="org-string">'Kf_M'</span>, args.Kf_M, ...
|
|
<span class="org-string">'Cf_M'</span>, args.Cf_M, ...
|
|
<span class="org-string">'Kt_M'</span>, args.Kt_M, ...
|
|
<span class="org-string">'Ct_M'</span>, args.Ct_M, ...
|
|
<span class="org-string">'Kf_F'</span>, args.Kf_F, ...
|
|
<span class="org-string">'Cf_F'</span>, args.Cf_F, ...
|
|
<span class="org-string">'Kt_F'</span>, args.Kt_F, ...
|
|
<span class="org-string">'Ct_F'</span>, args.Ct_F, ...
|
|
<span class="org-string">'Ka_F'</span>, args.Ka_F, ...
|
|
<span class="org-string">'Ca_F'</span>, args.Ca_F, ...
|
|
<span class="org-string">'Kr_F'</span>, args.Kr_F, ...
|
|
<span class="org-string">'Cr_F'</span>, args.Cr_F, ...
|
|
<span class="org-string">'Ka_M'</span>, args.Ka_M, ...
|
|
<span class="org-string">'Ca_M'</span>, args.Ca_M, ...
|
|
<span class="org-string">'Kr_M'</span>, args.Kr_M, ...
|
|
<span class="org-string">'Cr_M'</span>, args.Cr_M);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeCylindricalPlatforms(stewart, <span class="org-string">'Fpm'</span>, args.Fpm, <span class="org-string">'Fph'</span>, args.Fph, <span class="org-string">'Fpr'</span>, args.Fpr, <span class="org-string">'Mpm'</span>, args.Mpm, <span class="org-string">'Mph'</span>, args.Mph, <span class="org-string">'Mpr'</span>, args.Mpr);
|
|
|
|
stewart = initializeCylindricalStruts(stewart, <span class="org-string">'Fsm'</span>, args.Fsm, <span class="org-string">'Fsh'</span>, args.Fsh, <span class="org-string">'Fsr'</span>, args.Fsr, <span class="org-string">'Msm'</span>, args.Msm, <span class="org-string">'Msh'</span>, args.Msh, <span class="org-string">'Msr'</span>, args.Msr);
|
|
|
|
stewart = computeJacobian(stewart);
|
|
|
|
stewart = initializeStewartPose(stewart, <span class="org-string">'AP'</span>, args.AP, <span class="org-string">'ARB'</span>, args.ARB);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'accelerometer'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Equilibrium position of the each joint.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">
|
|
<span class="org-keyword">if</span> args.Foffset <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'none'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'rigid'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'init'</span>)
|
|
load(<span class="org-string">'mat/Foffset.mat'</span>, <span class="org-string">'Fnm'</span>);
|
|
stewart.actuators.dLeq = <span class="org-type">-</span>Fnm<span class="org-type">'./</span>stewart.Ki;
|
|
<span class="org-keyword">else</span>
|
|
stewart.actuators.dLeq = args.dLeq;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc446fb4" class="outline-3">
|
|
<h3 id="orgc446fb4">Add Type</h3>
|
|
<div class="outline-text-3" id="text-orgc446fb4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
stewart.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
stewart.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
stewart.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'init'</span>
|
|
stewart.type = 4;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc4b3b65" class="outline-3">
|
|
<h3 id="orgc4b3b65">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-orgc4b3b65">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> nano_hexapod = stewart;
|
|
save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'nano_hexapod'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbc058d5" class="outline-2">
|
|
<h2 id="orgbc058d5"><span class="section-number-2">12</span> Sample</h2>
|
|
<div class="outline-text-2" id="text-12">
|
|
<p>
|
|
<a id="org61d7d29"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org83ab8d1" class="outline-3">
|
|
<h3 id="org83ab8d1">Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-org83ab8d1">
|
|
<p>
|
|
The Simscape model of the sample environment is composed of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>A rigid transform that can be used to translate the sample (position offset)</li>
|
|
<li>A cartesian joint to add some flexibility to the sample environment mount</li>
|
|
<li>A solid that represent the sample</li>
|
|
<li>An input is added to apply some external forces and torques at the center of the sample environment.
|
|
This could be the case for cable forces for instance.</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orga1539b2" class="figure">
|
|
<p><img src="figs/images/simscape_model_sample.png" alt="simscape_model_sample.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 20: </span>Simscape model for the Sample</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgfaee415" class="figure">
|
|
<p><img src="figs/images/simscape_picture_sample.png" alt="simscape_picture_sample.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 21: </span>Simscape picture for the Sample</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org97e8fa7" class="outline-3">
|
|
<h3 id="org97e8fa7">Function description</h3>
|
|
<div class="outline-text-3" id="text-org97e8fa7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[sample]</span> = <span class="org-function-name">initializeSample</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgee69310" class="outline-3">
|
|
<h3 id="orgee69310">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgee69310">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'rigid'</span>, <span class="org-string">'flexible'</span>, <span class="org-string">'none'</span>, <span class="org-string">'init'</span>})} = <span class="org-string">'flexible'</span>
|
|
<span class="org-variable-name">args</span>.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 <span class="org-comment">% [m]</span>
|
|
<span class="org-variable-name">args</span>.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 <span class="org-comment">% [m]</span>
|
|
<span class="org-variable-name">args</span>.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 <span class="org-comment">% [kg]</span>
|
|
<span class="org-variable-name">args</span>.freq (6,1) double {mustBeNumeric, mustBePositive} = 100<span class="org-type">*</span>ones(6,1) <span class="org-comment">% [Hz]</span>
|
|
<span class="org-variable-name">args</span>.offset (1,1) double {mustBeNumeric} = 0 <span class="org-comment">% [m]</span>
|
|
<span class="org-variable-name">args</span>.Foffset logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5c6a4c0" class="outline-3">
|
|
<h3 id="org5c6a4c0">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-org5c6a4c0">
|
|
<p>
|
|
First, we initialize the <code>sample</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> sample = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7211784" class="outline-3">
|
|
<h3 id="org7211784">Add Sample Type</h3>
|
|
<div class="outline-text-3" id="text-org7211784">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'none'</span>
|
|
sample.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rigid'</span>
|
|
sample.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'flexible'</span>
|
|
sample.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'init'</span>
|
|
sample.type = 3;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb205d51" class="outline-3">
|
|
<h3 id="orgb205d51">Material and Geometry</h3>
|
|
<div class="outline-text-3" id="text-orgb205d51">
|
|
<p>
|
|
We define the geometrical parameters of the sample as well as its mass and position.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> sample.radius = args.radius; <span class="org-comment">% [m]</span>
|
|
sample.height = args.height; <span class="org-comment">% [m]</span>
|
|
sample.mass = args.mass; <span class="org-comment">% [kg]</span>
|
|
sample.offset = args.offset; <span class="org-comment">% [m]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc8a58fa" class="outline-3">
|
|
<h3 id="orgc8a58fa">Compute the Inertia</h3>
|
|
<div class="outline-text-3" id="text-orgc8a58fa">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> sample.inertia = [1<span class="org-type">/</span>12 <span class="org-type">*</span> sample.mass <span class="org-type">*</span> (3<span class="org-type">*</span>sample.radius<span class="org-type">^</span>2 <span class="org-type">+</span> sample.height<span class="org-type">^</span>2); ...
|
|
1<span class="org-type">/</span>12 <span class="org-type">*</span> sample.mass <span class="org-type">*</span> (3<span class="org-type">*</span>sample.radius<span class="org-type">^</span>2 <span class="org-type">+</span> sample.height<span class="org-type">^</span>2); ...
|
|
1<span class="org-type">/</span>2 <span class="org-type">*</span> sample.mass <span class="org-type">*</span> sample.radius<span class="org-type">^</span>2];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org112cacd" class="outline-3">
|
|
<h3 id="org112cacd">Stiffness and Damping properties</h3>
|
|
<div class="outline-text-3" id="text-org112cacd">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> sample.K = zeros(6, 1);
|
|
sample.C = zeros(6, 1);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Translation Stiffness and Damping:
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> sample.K(1<span class="org-type">:</span>3) = sample.mass <span class="org-type">.*</span> (2<span class="org-type">*</span><span class="org-constant">pi</span> <span class="org-type">*</span> args.freq(1<span class="org-type">:</span>3))<span class="org-type">.^</span>2; <span class="org-comment">% [N/m]</span>
|
|
sample.C(1<span class="org-type">:</span>3) = 0.1 <span class="org-type">*</span> sqrt(sample.K(1<span class="org-type">:</span>3)<span class="org-type">*</span>sample.mass); <span class="org-comment">% [N/(m/s)]</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Rotational Stiffness and Damping:
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> sample.K(4<span class="org-type">:</span>6) = sample.inertia <span class="org-type">.*</span> (2<span class="org-type">*</span><span class="org-constant">pi</span> <span class="org-type">*</span> args.freq(4<span class="org-type">:</span>6))<span class="org-type">.^</span>2; <span class="org-comment">% [N/m]</span>
|
|
sample.C(4<span class="org-type">:</span>6) = 0.1 <span class="org-type">*</span> sqrt(sample.K(4<span class="org-type">:</span>6)<span class="org-type">.*</span>sample.inertia); <span class="org-comment">% [N/(m/s)]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org27401d0" class="outline-3">
|
|
<h3 id="org27401d0">Equilibrium position of the each joint.</h3>
|
|
<div class="outline-text-3" id="text-org27401d0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Foffset <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'none'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'rigid'</span>) <span class="org-type">&&</span> <span class="org-type">~</span>strcmp(args.type, <span class="org-string">'init'</span>)
|
|
load(<span class="org-string">'mat/Foffset.mat'</span>, <span class="org-string">'Fsm'</span>);
|
|
sample.Deq = <span class="org-type">-</span>Fsm<span class="org-type">'./</span>sample.K;
|
|
<span class="org-keyword">else</span>
|
|
sample.Deq = zeros(6,1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8c3f548" class="outline-3">
|
|
<h3 id="org8c3f548">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-org8c3f548">
|
|
<p>
|
|
The <code>sample</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'sample'</span>, <span class="org-string">'-append'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org08f6708" class="outline-2">
|
|
<h2 id="org08f6708"><span class="section-number-2">13</span> Initialize Controller</h2>
|
|
<div class="outline-text-2" id="text-13">
|
|
<p>
|
|
<a id="org2eedf8b"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcd721e8" class="outline-3">
|
|
<h3 id="orgcd721e8">Function Declaration and Documentation</h3>
|
|
<div class="outline-text-3" id="text-orgcd721e8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">initializeController</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb1de776" class="outline-3">
|
|
<h3 id="orgb1de776">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgb1de776">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.type char {mustBeMember(args.type,{<span class="org-string">'open-loop'</span>, <span class="org-string">'iff'</span>, <span class="org-string">'dvf'</span>, <span class="org-string">'hac-dvf'</span>, <span class="org-string">'ref-track-L'</span>, <span class="org-string">'ref-track-iff-L'</span>, <span class="org-string">'cascade-hac-lac'</span>, <span class="org-string">'hac-iff'</span>, <span class="org-string">'stabilizing'</span>})} = <span class="org-string">'open-loop'</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc6bb2bb" class="outline-3">
|
|
<h3 id="orgc6bb2bb">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-orgc6bb2bb">
|
|
<p>
|
|
First, we initialize the <code>controller</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> controller = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf0db064" class="outline-3">
|
|
<h3 id="orgf0db064">Controller Type</h3>
|
|
<div class="outline-text-3" id="text-orgf0db064">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'open-loop'</span>
|
|
controller.type = 1;
|
|
controller.name = <span class="org-string">'Open-Loop'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'dvf'</span>
|
|
controller.type = 2;
|
|
controller.name = <span class="org-string">'Decentralized Direct Velocity Feedback'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'iff'</span>
|
|
controller.type = 3;
|
|
controller.name = <span class="org-string">'Decentralized Integral Force Feedback'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'hac-dvf'</span>
|
|
controller.type = 4;
|
|
controller.name = <span class="org-string">'HAC-DVF'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'ref-track-L'</span>
|
|
controller.type = 5;
|
|
controller.name = <span class="org-string">'Reference Tracking in the frame of the legs'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'ref-track-iff-L'</span>
|
|
controller.type = 6;
|
|
controller.name = <span class="org-string">'Reference Tracking in the frame of the legs + IFF'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'cascade-hac-lac'</span>
|
|
controller.type = 7;
|
|
controller.name = <span class="org-string">'Cascade Control + HAC-LAC'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'hac-iff'</span>
|
|
controller.type = 8;
|
|
controller.name = <span class="org-string">'HAC-IFF'</span>;
|
|
<span class="org-keyword">case</span> <span class="org-string">'stabilizing'</span>
|
|
controller.type = 9;
|
|
controller.name = <span class="org-string">'Stabilizing Controller'</span>;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org443e57e" class="outline-3">
|
|
<h3 id="org443e57e">Save the Structure</h3>
|
|
<div class="outline-text-3" id="text-org443e57e">
|
|
<p>
|
|
The <code>controller</code> structure is saved.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/controller.mat'</span>, <span class="org-string">'controller'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc5f188e" class="outline-2">
|
|
<h2 id="orgc5f188e"><span class="section-number-2">14</span> Generate Reference Signals</h2>
|
|
<div class="outline-text-2" id="text-14">
|
|
<p>
|
|
<a id="orgcfae74e"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdef7115" class="outline-3">
|
|
<h3 id="orgdef7115">Function Declaration and Documentation</h3>
|
|
<div class="outline-text-3" id="text-orgdef7115">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[ref]</span> = <span class="org-function-name">initializeReferences</span>(<span class="org-variable-name">args</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga1c48e4" class="outline-3">
|
|
<h3 id="orga1c48e4">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orga1c48e4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-comment">% Sampling Frequency [s]</span>
|
|
<span class="org-variable-name">args</span>.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e<span class="org-type">-</span>3
|
|
<span class="org-comment">% Maximum simulation time [s]</span>
|
|
<span class="org-variable-name">args</span>.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
|
|
<span class="org-comment">% Either "constant" / "triangular" / "sinusoidal"</span>
|
|
<span class="org-variable-name">args</span>.Dy_type char {mustBeMember(args.Dy_type,{<span class="org-string">'constant'</span>, <span class="org-string">'triangular'</span>, <span class="org-string">'sinusoidal'</span>})} = <span class="org-string">'constant'</span>
|
|
<span class="org-comment">% Amplitude of the displacement [m]</span>
|
|
<span class="org-variable-name">args</span>.Dy_amplitude (1,1) double {mustBeNumeric} = 0
|
|
<span class="org-comment">% Period of the displacement [s]</span>
|
|
<span class="org-variable-name">args</span>.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-comment">% Either "constant" / "triangular" / "sinusoidal"</span>
|
|
<span class="org-variable-name">args</span>.Ry_type char {mustBeMember(args.Ry_type,{<span class="org-string">'constant'</span>, <span class="org-string">'triangular'</span>, <span class="org-string">'sinusoidal'</span>})} = <span class="org-string">'constant'</span>
|
|
<span class="org-comment">% Amplitude [rad]</span>
|
|
<span class="org-variable-name">args</span>.Ry_amplitude (1,1) double {mustBeNumeric} = 0
|
|
<span class="org-comment">% Period of the displacement [s]</span>
|
|
<span class="org-variable-name">args</span>.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-comment">% Either "constant" / "rotating"</span>
|
|
<span class="org-variable-name">args</span>.Rz_type char {mustBeMember(args.Rz_type,{<span class="org-string">'constant'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'rotating-not-filtered'</span>})} = <span class="org-string">'constant'</span>
|
|
<span class="org-comment">% Initial angle [rad]</span>
|
|
<span class="org-variable-name">args</span>.Rz_amplitude (1,1) double {mustBeNumeric} = 0
|
|
<span class="org-comment">% Period of the rotating [s]</span>
|
|
<span class="org-variable-name">args</span>.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
<span class="org-comment">% For now, only constant is implemented</span>
|
|
<span class="org-variable-name">args</span>.Dh_type char {mustBeMember(args.Dh_type,{<span class="org-string">'constant'</span>})} = <span class="org-string">'constant'</span>
|
|
<span class="org-comment">% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)</span>
|
|
<span class="org-variable-name">args</span>.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
|
|
<span class="org-comment">% For now, only constant is implemented</span>
|
|
<span class="org-variable-name">args</span>.Rm_type char {mustBeMember(args.Rm_type,{<span class="org-string">'constant'</span>})} = <span class="org-string">'constant'</span>
|
|
<span class="org-comment">% Initial position of the two masses</span>
|
|
<span class="org-variable-name">args</span>.Rm_pos (2,1) double {mustBeNumeric} = [0; <span class="org-constant">pi</span>]
|
|
<span class="org-comment">% For now, only constant is implemented</span>
|
|
<span class="org-variable-name">args</span>.Dn_type char {mustBeMember(args.Dn_type,{<span class="org-string">'constant'</span>})} = <span class="org-string">'constant'</span>
|
|
<span class="org-comment">% Initial position [m,m,m,rad,rad,rad] of the top platform</span>
|
|
<span class="org-variable-name">args</span>.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-orgb2488be" class="outline-3">
|
|
<h3 id="orgb2488be">Initialize Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgb2488be">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Set Sampling Time</span></span>
|
|
Ts = args.Ts;
|
|
Tmax = args.Tmax;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Low Pass Filter to filter out the references</span></span>
|
|
s = zpk(<span class="org-string">'s'</span>);
|
|
w0 = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>10;
|
|
xi = 1;
|
|
H_lpf = 1<span class="org-type">/</span>(1 <span class="org-type">+</span> 2<span class="org-type">*</span>xi<span class="org-type">/</span>w0<span class="org-type">*</span>s <span class="org-type">+</span> s<span class="org-type">^</span>2<span class="org-type">/</span>w0<span class="org-type">^</span>2);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc0941f6" class="outline-3">
|
|
<h3 id="orgc0941f6">Translation Stage</h3>
|
|
<div class="outline-text-3" id="text-orgc0941f6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Translation stage - Dy</span></span>
|
|
t = 0<span class="org-type">:</span>Ts<span class="org-type">:</span>Tmax; <span class="org-comment">% Time Vector [s]</span>
|
|
Dy = zeros(length(t), 1);
|
|
Dyd = zeros(length(t), 1);
|
|
Dydd = zeros(length(t), 1);
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Dy_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Dy<span class="org-type">(:) </span>= args.Dy_amplitude;
|
|
Dyd<span class="org-type">(:) </span>= 0;
|
|
Dydd<span class="org-type">(:) </span>= 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'triangular'</span>
|
|
<span class="org-comment">% This is done to unsure that we start with no displacement</span>
|
|
Dy_raw = args.Dy_amplitude<span class="org-type">*</span>sawtooth(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t<span class="org-type">/</span>args.Dy_period,1<span class="org-type">/</span>2);
|
|
i0 = find(t<span class="org-type">>=</span>args.Dy_period<span class="org-type">/</span>4,1);
|
|
Dy(1<span class="org-type">:</span>end<span class="org-type">-</span>i0<span class="org-type">+</span>1) = Dy_raw(i0<span class="org-type">:</span>end);
|
|
Dy(end<span class="org-type">-</span>i0<span class="org-type">+</span>2<span class="org-type">:</span>end) = Dy_raw(end); <span class="org-comment">% we fix the last value</span>
|
|
|
|
<span class="org-comment">% The signal is filtered out</span>
|
|
Dy = lsim(H_lpf, Dy, t);
|
|
Dyd = lsim(H_lpf<span class="org-type">*</span>s, Dy, t);
|
|
Dydd = lsim(H_lpf<span class="org-type">*</span>s<span class="org-type">^</span>2, Dy, t);
|
|
<span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span>
|
|
Dy<span class="org-type">(:) </span>= args.Dy_amplitude<span class="org-type">*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Dy_period<span class="org-type">*</span>t);
|
|
Dyd = args.Dy_amplitude<span class="org-type">*</span>2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Dy_period<span class="org-type">*</span>cos(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Dy_period<span class="org-type">*</span>t);
|
|
Dydd = <span class="org-type">-</span>args.Dy_amplitude<span class="org-type">*</span>(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Dy_period)<span class="org-type">^</span>2<span class="org-type">*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Dy_period<span class="org-type">*</span>t);
|
|
<span class="org-keyword">otherwise</span>
|
|
warning(<span class="org-string">'Dy_type is not set correctly'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
Dy = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Dy), <span class="org-string">'deriv'</span>, Dyd, <span class="org-string">'dderiv'</span>, Dydd);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org148b614" class="outline-3">
|
|
<h3 id="org148b614">Tilt Stage</h3>
|
|
<div class="outline-text-3" id="text-org148b614">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Tilt Stage - Ry</span></span>
|
|
t = 0<span class="org-type">:</span>Ts<span class="org-type">:</span>Tmax; <span class="org-comment">% Time Vector [s]</span>
|
|
Ry = zeros(length(t), 1);
|
|
Ryd = zeros(length(t), 1);
|
|
Rydd = zeros(length(t), 1);
|
|
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Ry_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Ry<span class="org-type">(:) </span>= args.Ry_amplitude;
|
|
Ryd<span class="org-type">(:) </span>= 0;
|
|
Rydd<span class="org-type">(:) </span>= 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'triangular'</span>
|
|
Ry_raw = args.Ry_amplitude<span class="org-type">*</span>sawtooth(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t<span class="org-type">/</span>args.Ry_period,1<span class="org-type">/</span>2);
|
|
i0 = find(t<span class="org-type">>=</span>args.Ry_period<span class="org-type">/</span>4,1);
|
|
Ry(1<span class="org-type">:</span>end<span class="org-type">-</span>i0<span class="org-type">+</span>1) = Ry_raw(i0<span class="org-type">:</span>end);
|
|
Ry(end<span class="org-type">-</span>i0<span class="org-type">+</span>2<span class="org-type">:</span>end) = Ry_raw(end); <span class="org-comment">% we fix the last value</span>
|
|
|
|
<span class="org-comment">% The signal is filtered out</span>
|
|
Ry = lsim(H_lpf, Ry, t);
|
|
Ryd = lsim(H_lpf<span class="org-type">*</span>s, Ry, t);
|
|
Rydd = lsim(H_lpf<span class="org-type">*</span>s<span class="org-type">^</span>2, Ry, t);
|
|
<span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span>
|
|
Ry<span class="org-type">(:) </span>= args.Ry_amplitude<span class="org-type">*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Ry_period<span class="org-type">*</span>t);
|
|
|
|
Ryd = args.Ry_amplitude<span class="org-type">*</span>2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Ry_period<span class="org-type">*</span>cos(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Ry_period<span class="org-type">*</span>t);
|
|
Rydd = <span class="org-type">-</span>args.Ry_amplitude<span class="org-type">*</span>(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Ry_period)<span class="org-type">^</span>2<span class="org-type">*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Ry_period<span class="org-type">*</span>t);
|
|
<span class="org-keyword">otherwise</span>
|
|
warning(<span class="org-string">'Ry_type is not set correctly'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
Ry = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Ry), <span class="org-string">'deriv'</span>, Ryd, <span class="org-string">'dderiv'</span>, Rydd);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org591aa90" class="outline-3">
|
|
<h3 id="org591aa90">Spindle</h3>
|
|
<div class="outline-text-3" id="text-org591aa90">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Spindle - Rz</span></span>
|
|
t = 0<span class="org-type">:</span>Ts<span class="org-type">:</span>Tmax; <span class="org-comment">% Time Vector [s]</span>
|
|
Rz = zeros(length(t), 1);
|
|
Rzd = zeros(length(t), 1);
|
|
Rzdd = zeros(length(t), 1);
|
|
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Rz_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Rz<span class="org-type">(:) </span>= args.Rz_amplitude;
|
|
Rzd<span class="org-type">(:) </span>= 0;
|
|
Rzdd<span class="org-type">(:) </span>= 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'rotating-not-filtered'</span>
|
|
Rz<span class="org-type">(:) </span>= 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Rz_period<span class="org-type">*</span>t;
|
|
|
|
<span class="org-comment">% The signal is filtered out</span>
|
|
Rz<span class="org-type">(:) </span>= 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Rz_period<span class="org-type">*</span>t;
|
|
Rzd<span class="org-type">(:) </span>= 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Rz_period;
|
|
Rzdd<span class="org-type">(:) </span>= 0;
|
|
|
|
<span class="org-comment">% We add the angle offset</span>
|
|
Rz = Rz <span class="org-type">+</span> args.Rz_amplitude;
|
|
|
|
<span class="org-keyword">case</span> <span class="org-string">'rotating'</span>
|
|
Rz<span class="org-type">(:) </span>= 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>args.Rz_period<span class="org-type">*</span>t;
|
|
|
|
<span class="org-comment">% The signal is filtered out</span>
|
|
Rz = lsim(H_lpf, Rz, t);
|
|
Rzd = lsim(H_lpf<span class="org-type">*</span>s, Rz, t);
|
|
Rzdd = lsim(H_lpf<span class="org-type">*</span>s<span class="org-type">^</span>2, Rz, t);
|
|
|
|
<span class="org-comment">% We add the angle offset</span>
|
|
Rz = Rz <span class="org-type">+</span> args.Rz_amplitude;
|
|
<span class="org-keyword">otherwise</span>
|
|
warning(<span class="org-string">'Rz_type is not set correctly'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
Rz = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Rz), <span class="org-string">'deriv'</span>, Rzd, <span class="org-string">'dderiv'</span>, Rzdd);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5fca80a" class="outline-3">
|
|
<h3 id="org5fca80a">Micro Hexapod</h3>
|
|
<div class="outline-text-3" id="text-org5fca80a">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Micro-Hexapod</span></span>
|
|
t = [0, Ts];
|
|
Dh = zeros(length(t), 6);
|
|
Dhl = zeros(length(t), 6);
|
|
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Dh_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Dh = [args.Dh_pos, args.Dh_pos];
|
|
|
|
load(<span class="org-string">'mat/stages.mat'</span>, <span class="org-string">'micro_hexapod'</span>);
|
|
|
|
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
|
|
|
|
tx = args.Dh_pos(4);
|
|
ty = args.Dh_pos(5);
|
|
tz = args.Dh_pos(6);
|
|
|
|
ARB = [cos(tz) <span class="org-type">-</span>sin(tz) 0;
|
|
sin(tz) cos(tz) 0;
|
|
0 0 1]<span class="org-type">*</span>...
|
|
[ cos(ty) 0 sin(ty);
|
|
0 1 0;
|
|
<span class="org-type">-</span>sin(ty) 0 cos(ty)]<span class="org-type">*</span>...
|
|
[1 0 0;
|
|
0 cos(tx) <span class="org-type">-</span>sin(tx);
|
|
0 sin(tx) cos(tx)];
|
|
|
|
[<span class="org-type">~</span>, Dhl] = inverseKinematics(micro_hexapod, <span class="org-string">'AP'</span>, AP, <span class="org-string">'ARB'</span>, ARB);
|
|
Dhl = [Dhl, Dhl];
|
|
<span class="org-keyword">otherwise</span>
|
|
warning(<span class="org-string">'Dh_type is not set correctly'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
Dh = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Dh));
|
|
Dhl = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Dhl));
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc8390ff" class="outline-3">
|
|
<h3 id="orgc8390ff">Axis Compensation</h3>
|
|
<div class="outline-text-3" id="text-orgc8390ff">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Axis Compensation - Rm</span></span>
|
|
t = [0, Ts];
|
|
|
|
Rm = [args.Rm_pos, args.Rm_pos];
|
|
Rm = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Rm));
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5ddf967" class="outline-3">
|
|
<h3 id="org5ddf967">Nano Hexapod</h3>
|
|
<div class="outline-text-3" id="text-org5ddf967">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Nano-Hexapod</span></span>
|
|
t = [0, Ts];
|
|
Dn = zeros(length(t), 6);
|
|
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Dn_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
Dn = [args.Dn_pos, args.Dn_pos];
|
|
|
|
load(<span class="org-string">'mat/stages.mat'</span>, <span class="org-string">'nano_hexapod'</span>);
|
|
|
|
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
|
|
|
|
tx = args.Dn_pos(4);
|
|
ty = args.Dn_pos(5);
|
|
tz = args.Dn_pos(6);
|
|
|
|
ARB = [cos(tz) <span class="org-type">-</span>sin(tz) 0;
|
|
sin(tz) cos(tz) 0;
|
|
0 0 1]<span class="org-type">*</span>...
|
|
[ cos(ty) 0 sin(ty);
|
|
0 1 0;
|
|
<span class="org-type">-</span>sin(ty) 0 cos(ty)]<span class="org-type">*</span>...
|
|
[1 0 0;
|
|
0 cos(tx) <span class="org-type">-</span>sin(tx);
|
|
0 sin(tx) cos(tx)];
|
|
|
|
[<span class="org-type">~</span>, Dnl] = inverseKinematics(nano_hexapod, <span class="org-string">'AP'</span>, AP, <span class="org-string">'ARB'</span>, ARB);
|
|
Dnl = [Dnl, Dnl];
|
|
<span class="org-keyword">otherwise</span>
|
|
warning(<span class="org-string">'Dn_type is not set correctly'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
Dn = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Dn));
|
|
Dnl = struct(<span class="org-string">'time'</span>, t, <span class="org-string">'signals'</span>, struct(<span class="org-string">'values'</span>, Dnl));
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgac26a7c" class="outline-3">
|
|
<h3 id="orgac26a7c">Save</h3>
|
|
<div class="outline-text-3" id="text-orgac26a7c">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save(<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">'Dhl'</span>, <span class="org-string">'Rm'</span>, <span class="org-string">'Dn'</span>, <span class="org-string">'Dnl'</span>, <span class="org-string">'args'</span>, <span class="org-string">'Ts'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8106d91" class="outline-2">
|
|
<h2 id="org8106d91"><span class="section-number-2">15</span> Initialize Disturbances</h2>
|
|
<div class="outline-text-2" id="text-15">
|
|
<p>
|
|
<a id="orgbc283ce"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orge05ed22" class="outline-3">
|
|
<h3 id="orge05ed22">Function Declaration and Documentation</h3>
|
|
<div class="outline-text-3" id="text-orge05ed22">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">initializeDisturbances</span>(<span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeDisturbances - Initialize the disturbances</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [] = initializeDisturbances(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args -</span>
|
|
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbb2f4ad" class="outline-3">
|
|
<h3 id="orgbb2f4ad">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-orgbb2f4ad">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-comment">% Global parameter to enable or disable the disturbances</span>
|
|
<span class="org-variable-name">args</span>.enable logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-comment">% Ground Motion - X direction</span>
|
|
<span class="org-variable-name">args</span>.Dwx logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-comment">% Ground Motion - Y direction</span>
|
|
<span class="org-variable-name">args</span>.Dwy logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-comment">% Ground Motion - Z direction</span>
|
|
<span class="org-variable-name">args</span>.Dwz logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-comment">% Translation Stage - X direction</span>
|
|
<span class="org-variable-name">args</span>.Fty_x logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-comment">% Translation Stage - Z direction</span>
|
|
<span class="org-variable-name">args</span>.Fty_z logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-comment">% Spindle - Z direction</span>
|
|
<span class="org-variable-name">args</span>.Frz_z logical {mustBeNumericOrLogical} = <span class="org-constant">true</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-orgb224bf9" class="outline-3">
|
|
<h3 id="orgb224bf9">Load Data</h3>
|
|
<div class="outline-text-3" id="text-orgb224bf9">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/dist_psd.mat'</span>, <span class="org-string">'dist_f'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We remove the first frequency point that usually is very large.
|
|
</p>
|
|
</div>
|
|
</div>
|
|
<div id="outline-container-org4c390fe" class="outline-3">
|
|
<h3 id="org4c390fe">Parameters</h3>
|
|
<div class="outline-text-3" id="text-org4c390fe">
|
|
<p>
|
|
We define some parameters that will be used in the algorithm.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> Fs = 2<span class="org-type">*</span>dist_f.f(end); <span class="org-comment">% Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]</span>
|
|
N = 2<span class="org-type">*</span>length(dist_f.f); <span class="org-comment">% Number of Samples match the one of the wanted PSD</span>
|
|
T0 = N<span class="org-type">/</span>Fs; <span class="org-comment">% Signal Duration [s]</span>
|
|
df = 1<span class="org-type">/</span>T0; <span class="org-comment">% Frequency resolution of the DFT [Hz]</span>
|
|
<span class="org-comment">% Also equal to (dist_f.f(2)-dist_f.f(1))</span>
|
|
t = linspace(0, T0, N<span class="org-type">+</span>1)<span class="org-type">'</span>; <span class="org-comment">% Time Vector [s]</span>
|
|
Ts = 1<span class="org-type">/</span>Fs; <span class="org-comment">% Sampling Time [s]</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2ea1ed6" class="outline-3">
|
|
<h3 id="org2ea1ed6">Ground Motion</h3>
|
|
<div class="outline-text-3" id="text-org2ea1ed6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> phi = dist_f.psd_gm;
|
|
C = zeros(N<span class="org-type">/</span>2,1);
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:N/2</span>
|
|
C(<span class="org-constant">i</span>) = sqrt(phi(<span class="org-constant">i</span>)<span class="org-type">*</span>df);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Dwx <span class="org-type">&&</span> args.enable
|
|
rng<span class="org-type">(111);</span>
|
|
theta = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>rand(N<span class="org-type">/</span>2,1); <span class="org-comment">% Generate random phase [rad]</span>
|
|
Cx = [0 ; C<span class="org-type">.*</span>complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2<span class="org-type">:</span>end)))];;
|
|
Dwx = N<span class="org-type">/</span>sqrt(2)<span class="org-type">*</span>ifft(Cx); <span class="org-comment">% Ground Motion - x direction [m]</span>
|
|
<span class="org-keyword">else</span>
|
|
Dwx = zeros(length(t), 1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Dwy <span class="org-type">&&</span> args.enable
|
|
rng<span class="org-type">(112);</span>
|
|
theta = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>rand(N<span class="org-type">/</span>2,1); <span class="org-comment">% Generate random phase [rad]</span>
|
|
Cx = [0 ; C<span class="org-type">.*</span>complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2<span class="org-type">:</span>end)))];;
|
|
Dwy = N<span class="org-type">/</span>sqrt(2)<span class="org-type">*</span>ifft(Cx); <span class="org-comment">% Ground Motion - y direction [m]</span>
|
|
<span class="org-keyword">else</span>
|
|
Dwy = zeros(length(t), 1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Dwy <span class="org-type">&&</span> args.enable
|
|
rng<span class="org-type">(113);</span>
|
|
theta = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>rand(N<span class="org-type">/</span>2,1); <span class="org-comment">% Generate random phase [rad]</span>
|
|
Cx = [0 ; C<span class="org-type">.*</span>complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2<span class="org-type">:</span>end)))];;
|
|
Dwz = N<span class="org-type">/</span>sqrt(2)<span class="org-type">*</span>ifft(Cx); <span class="org-comment">% Ground Motion - z direction [m]</span>
|
|
<span class="org-keyword">else</span>
|
|
Dwz = zeros(length(t), 1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb784e1f" class="outline-3">
|
|
<h3 id="orgb784e1f">Translation Stage - X direction</h3>
|
|
<div class="outline-text-3" id="text-orgb784e1f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Fty_x <span class="org-type">&&</span> args.enable
|
|
phi = dist_f.psd_ty; <span class="org-comment">% TODO - we take here the vertical direction which is wrong but approximate</span>
|
|
C = zeros(N<span class="org-type">/</span>2,1);
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:N/2</span>
|
|
C(<span class="org-constant">i</span>) = sqrt(phi(<span class="org-constant">i</span>)<span class="org-type">*</span>df);
|
|
<span class="org-keyword">end</span>
|
|
rng<span class="org-type">(121);</span>
|
|
theta = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>rand(N<span class="org-type">/</span>2,1); <span class="org-comment">% Generate random phase [rad]</span>
|
|
Cx = [0 ; C<span class="org-type">.*</span>complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2<span class="org-type">:</span>end)))];;
|
|
u = N<span class="org-type">/</span>sqrt(2)<span class="org-type">*</span>ifft(Cx); <span class="org-comment">% Disturbance Force Ty x [N]</span>
|
|
Fty_x = u;
|
|
<span class="org-keyword">else</span>
|
|
Fty_x = zeros(length(t), 1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf93582d" class="outline-3">
|
|
<h3 id="orgf93582d">Translation Stage - Z direction</h3>
|
|
<div class="outline-text-3" id="text-orgf93582d">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Fty_z <span class="org-type">&&</span> args.enable
|
|
phi = dist_f.psd_ty;
|
|
C = zeros(N<span class="org-type">/</span>2,1);
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:N/2</span>
|
|
C(<span class="org-constant">i</span>) = sqrt(phi(<span class="org-constant">i</span>)<span class="org-type">*</span>df);
|
|
<span class="org-keyword">end</span>
|
|
rng<span class="org-type">(122);</span>
|
|
theta = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>rand(N<span class="org-type">/</span>2,1); <span class="org-comment">% Generate random phase [rad]</span>
|
|
Cx = [0 ; C<span class="org-type">.*</span>complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2<span class="org-type">:</span>end)))];;
|
|
u = N<span class="org-type">/</span>sqrt(2)<span class="org-type">*</span>ifft(Cx); <span class="org-comment">% Disturbance Force Ty z [N]</span>
|
|
Fty_z = u;
|
|
<span class="org-keyword">else</span>
|
|
Fty_z = zeros(length(t), 1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgcf7bc46" class="outline-3">
|
|
<h3 id="orgcf7bc46">Spindle - Z direction</h3>
|
|
<div class="outline-text-3" id="text-orgcf7bc46">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.Frz_z <span class="org-type">&&</span> args.enable
|
|
phi = dist_f.psd_rz;
|
|
C = zeros(N<span class="org-type">/</span>2,1);
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:N/2</span>
|
|
C(<span class="org-constant">i</span>) = sqrt(phi(<span class="org-constant">i</span>)<span class="org-type">*</span>df);
|
|
<span class="org-keyword">end</span>
|
|
rng<span class="org-type">(131);</span>
|
|
theta = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>rand(N<span class="org-type">/</span>2,1); <span class="org-comment">% Generate random phase [rad]</span>
|
|
Cx = [0 ; C<span class="org-type">.*</span>complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2<span class="org-type">:</span>end)))];;
|
|
u = N<span class="org-type">/</span>sqrt(2)<span class="org-type">*</span>ifft(Cx); <span class="org-comment">% Disturbance Force Rz z [N]</span>
|
|
Frz_z = u;
|
|
<span class="org-keyword">else</span>
|
|
Frz_z = zeros(length(t), 1);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org105c318" class="outline-3">
|
|
<h3 id="org105c318">Direct Forces</h3>
|
|
<div class="outline-text-3" id="text-org105c318">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> u = zeros(length(t), 6);
|
|
Fd = u;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb4d9177" class="outline-3">
|
|
<h3 id="orgb4d9177">Set initial value to zero</h3>
|
|
<div class="outline-text-3" id="text-orgb4d9177">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> Dwx = Dwx <span class="org-type">-</span> Dwx(1);
|
|
Dwy = Dwy <span class="org-type">-</span> Dwy(1);
|
|
Dwz = Dwz <span class="org-type">-</span> Dwz(1);
|
|
Fty_x = Fty_x <span class="org-type">-</span> Fty_x(1);
|
|
Fty_z = Fty_z <span class="org-type">-</span> Fty_z(1);
|
|
Frz_z = Frz_z <span class="org-type">-</span> Frz_z(1);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgaf8c9f7" class="outline-3">
|
|
<h3 id="orgaf8c9f7">Save</h3>
|
|
<div class="outline-text-3" id="text-orgaf8c9f7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/nass_disturbances.mat'</span>, <span class="org-string">'Dwx'</span>, <span class="org-string">'Dwy'</span>, <span class="org-string">'Dwz'</span>, <span class="org-string">'Fty_x'</span>, <span class="org-string">'Fty_z'</span>, <span class="org-string">'Frz_z'</span>, <span class="org-string">'Fd'</span>, <span class="org-string">'Ts'</span>, <span class="org-string">'t'</span>, <span class="org-string">'args'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge4aa590" class="outline-2">
|
|
<h2 id="orge4aa590"><span class="section-number-2">16</span> Initialize Position Errors</h2>
|
|
<div class="outline-text-2" id="text-16">
|
|
<p>
|
|
<a id="org8a8f25c"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org3633d18" class="outline-3">
|
|
<h3 id="org3633d18">Function Declaration and Documentation</h3>
|
|
<div class="outline-text-3" id="text-org3633d18">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">initializePosError</span>(<span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializePosError - Initialize the position errors</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [] = initializePosError(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args -</span>
|
|
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org37567d0" class="outline-3">
|
|
<h3 id="org37567d0">Optional Parameters</h3>
|
|
<div class="outline-text-3" id="text-org37567d0">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.error logical {mustBeNumericOrLogical} = <span class="org-constant">false</span>
|
|
<span class="org-variable-name">args</span>.Dy (1,1) double {mustBeNumeric} = 0 <span class="org-comment">% [m]</span>
|
|
<span class="org-variable-name">args</span>.Ry (1,1) double {mustBeNumeric} = 0 <span class="org-comment">% [m]</span>
|
|
<span class="org-variable-name">args</span>.Rz (1,1) double {mustBeNumeric} = 0 <span class="org-comment">% [m]</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org51e7ef5" class="outline-3">
|
|
<h3 id="org51e7ef5">Structure initialization</h3>
|
|
<div class="outline-text-3" id="text-org51e7ef5">
|
|
<p>
|
|
First, we initialize the <code>pos_error</code> structure.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> pos_error = struct();
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7e10ff7" class="outline-3">
|
|
<h3 id="org7e10ff7">Type</h3>
|
|
<div class="outline-text-3" id="text-org7e10ff7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">if</span> args.error
|
|
pos_error.type = 1;
|
|
<span class="org-keyword">else</span>
|
|
pos_error.type = 0;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3fa3c9c" class="outline-3">
|
|
<h3 id="org3fa3c9c">Position Errors</h3>
|
|
<div class="outline-text-3" id="text-org3fa3c9c">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> pos_error.Dy = args.Dy;
|
|
pos_error.Ry = args.Ry;
|
|
pos_error.Rz = args.Rz;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgca6a79f" class="outline-3">
|
|
<h3 id="orgca6a79f">Save</h3>
|
|
<div class="outline-text-3" id="text-orgca6a79f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> save(<span class="org-string">'./mat/pos_error.mat'</span>, <span class="org-string">'pos_error'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org10272ac" class="outline-2">
|
|
<h2 id="org10272ac"><span class="section-number-2">17</span> Z-Axis Geophone</h2>
|
|
<div class="outline-text-2" id="text-17">
|
|
<p>
|
|
<a id="org0014c07"></a>
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[geophone]</span> = <span class="org-function-name">initializeZAxisGeophone</span>(<span class="org-variable-name">args</span>)
|
|
<span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e<span class="org-type">-</span>3 <span class="org-comment">% [kg]</span>
|
|
<span class="org-variable-name">args</span>.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 <span class="org-comment">% [Hz]</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
geophone.m = args.mass;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% The Stiffness is set to have the damping resonance frequency</span></span>
|
|
geophone.k = geophone.m <span class="org-type">*</span> (2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>args.freq)<span class="org-type">^</span>2;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% We set the damping value to have critical damping</span></span>
|
|
geophone.c = 2<span class="org-type">*</span>sqrt(geophone.m <span class="org-type">*</span> geophone.k);
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save(<span class="org-string">'./mat/geophone_z_axis.mat'</span>, <span class="org-string">'geophone'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga3cba43" class="outline-2">
|
|
<h2 id="orga3cba43"><span class="section-number-2">18</span> Z-Axis Accelerometer</h2>
|
|
<div class="outline-text-2" id="text-18">
|
|
<p>
|
|
<a id="orgbe2e0af"></a>
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[accelerometer]</span> = <span class="org-function-name">initializeZAxisAccelerometer</span>(<span class="org-variable-name">args</span>)
|
|
<span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e<span class="org-type">-</span>3 <span class="org-comment">% [kg]</span>
|
|
<span class="org-variable-name">args</span>.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 <span class="org-comment">% [Hz]</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
|
|
accelerometer.m = args.mass;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% The Stiffness is set to have the damping resonance frequency</span></span>
|
|
accelerometer.k = accelerometer.m <span class="org-type">*</span> (2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>args.freq)<span class="org-type">^</span>2;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% We set the damping value to have critical damping</span></span>
|
|
accelerometer.c = 2<span class="org-type">*</span>sqrt(accelerometer.m <span class="org-type">*</span> accelerometer.k);
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Gain correction of the accelerometer to have a unity gain until the resonance</span></span>
|
|
accelerometer.gain = <span class="org-type">-</span>accelerometer.k<span class="org-type">/</span>accelerometer.m;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
|
|
save(<span class="org-string">'./mat/accelerometer_z_axis.mat'</span>, <span class="org-string">'accelerometer'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2021-02-20 sam. 23:08</p>
|
|
</div>
|
|
</body>
|
|
</html>
|