From d24cddcc445bb19a5cab2140b520d6d5fd137517 Mon Sep 17 00:00:00 2001
From: Thomas Dehaeze <dehaeze.thomas@gmail.com>
Date: Tue, 1 Sep 2020 13:46:46 +0200
Subject: [PATCH] Add code to extract disturbance sensibility

---
 org/disturbances.org | 68 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 68 insertions(+)

diff --git a/org/disturbances.org b/org/disturbances.org
index deb9379..f712b6f 100644
--- a/org/disturbances.org
+++ b/org/disturbances.org
@@ -734,3 +734,71 @@ Let's now compare the effect of those perturbations on the position error of the
 #+name: fig:effect_disturbances_position_error
 #+caption: Effect of Perturbations on the position error ([[./figs/effect_disturbances_position_error.png][png]], [[./figs/effect_disturbances_position_error.pdf][pdf]])
 [[file:figs/effect_disturbances_position_error.png]]
+
+
+* TODO Sensibility to disturbances without the nano-hexapod         :noexport:
+#+begin_src matlab
+  initializeGround();
+  initializeGranite();
+  initializeTy();
+  initializeRy();
+  initializeRz();
+  initializeMicroHexapod();
+  initializeAxisc();
+  initializeMirror();
+  initializeNanoHexapod('type', 'rigid');
+#+end_src
+
+We set the stiffness of the payload fixation:
+#+begin_src matlab
+  Kp = 1e8; % [N/m]
+#+end_src
+
+Open Loop Control.
+#+begin_src matlab
+  initializeController('type', 'open-loop');
+#+end_src
+
+#+begin_src matlab
+  initializeSimscapeConfiguration('gravity', false);
+  initializeDisturbances('enable', false);
+  initializeLoggingConfiguration('log', 'none');
+#+end_src
+
+We identify the system for the following payload masses:
+#+begin_src matlab
+  Ms = [1, 10, 50];
+#+end_src
+
+#+begin_src matlab :exports none
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Micro-Hexapod
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz');   io_i = io_i + 1; % Z Ground motion
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z
+  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd');    io_i = io_i + 1; % Direct forces
+
+  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
+#+end_src
+
+#+begin_src matlab :exports none
+  Gd = {zeros(length(Ms), 1)};
+
+  for i = 1:length(Ms)
+      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
+
+      %% Run the linearization
+      G = linearize(mdl, io);
+      G.InputName  = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
+      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+      Gd(i) = {G};
+  end
+#+end_src
+
+#+begin_src matlab
+  save('./mat/Gd_ol.mat', 'Gd', 'Ms');
+#+end_src