tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Use new argument function validation technique

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-01-13 11:42:31 +01:00
parent 994fe2ccc7
commit a2917a50e9
20 changed files with 770 additions and 660 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

570
active_damping/index.org
View File

@ -97,7 +97,7 @@ The performance of this undamped system will be compared with the damped system
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
open('active_damping/matlab/sim_nano_station_id.slx') open('active_damping/matlab/sim_nass_active_damping.slx')
#+end_src #+end_src
** Init ** Init
@ -114,8 +114,8 @@ The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 1);
#+end_src #+end_src
All the controllers are set to 0. All the controllers are set to 0.
@ -124,16 +124,321 @@ All the controllers are set to 0.
save('./mat/controllers.mat', 'K', '-append'); save('./mat/controllers.mat', 'K', '-append');
K_iff = tf(zeros(6)); K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append'); save('./mat/controllers.mat', 'K_iff', '-append');
K_rmc = tf(zeros(6));
save('./mat/controllers.mat', 'K_rmc', '-append');
K_dvf = tf(zeros(6)); K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append'); save('./mat/controllers.mat', 'K_dvf', '-append');
#+end_src #+end_src
** Identification ** Identification of the transfer function from disturbance to position error
We identify the various transfer functions of the system
#+begin_src matlab #+begin_src matlab
G = identifyPlant(); %% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nass_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwx'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwy'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Dwx', 'Dwy', 'Dwz'};
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G('Edx', 'Dwx'), freqs, 'Hz'))), 'DisplayName', 'x');
plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dwz'), freqs, 'Hz'))), 'DisplayName', 'y');
plot(freqs, abs(squeeze(freqresp(G('Edz', 'Dwz'), freqs, 'Hz'))), 'DisplayName', 'z');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
legend('location', 'southeast')
#+end_src
** Identification of the plant
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nass_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
G_cart = G*inv(nano_hexapod.J');
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$T_{y}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$T_{z}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest')
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest')
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
** test
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nass_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Micro-Station/Dy'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Dy'};
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest')
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest')
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
** test on hexapod
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'test_nano_hexapod';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'x', 'y', 'z'};
#+end_src
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'test_nano_hexapod';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fx'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Fx'};
G.OutputName = {'x', 'y', 'z'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest')
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest')
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
** Identification of the dynamics for Active Damping
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nass_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
#+end_src #+end_src
And we save it for further analysis. And we save it for further analysis.
@ -141,6 +446,63 @@ And we save it for further analysis.
save('./active_damping/mat/plants.mat', 'G', '-append'); save('./active_damping/mat/plants.mat', 'G', '-append');
#+end_src #+end_src
** Plant for Active Damping
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
** Sensitivity to disturbances ** Sensitivity to disturbances
The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undamped]]. The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undamped]].
@ -242,6 +604,7 @@ The "plant" (transfer function from forces applied by the nano-hexapod to the me
#+CAPTION: Transfer Function from cartesian forces to displacement for the undamped plant ([[./figs/plant_undamped.png][png]], [[./figs/plant_undamped.pdf][pdf]]) #+CAPTION: Transfer Function from cartesian forces to displacement for the undamped plant ([[./figs/plant_undamped.png][png]], [[./figs/plant_undamped.pdf][pdf]])
[[file:figs/plant_undamped.png]] [[file:figs/plant_undamped.png]]
** Tomography Experiment
* Integral Force Feedback * Integral Force Feedback
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle matlab/iff.m :header-args:matlab+: :tangle matlab/iff.m
@ -269,185 +632,6 @@ Integral Force Feedback is applied.
In section [[sec:iff_1dof]], IFF is applied on a uni-axial system to understand its behavior. In section [[sec:iff_1dof]], IFF is applied on a uni-axial system to understand its behavior.
Then, it is applied on the simscape model. Then, it is applied on the simscape model.
** One degree-of-freedom example
:PROPERTIES:
:header-args:matlab+: :tangle no
:END:
<<sec:iff_1dof>>
*** Equations
#+begin_src latex :file iff_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
\begin{tikzpicture}
% Ground
\draw (-1, 0) -- (1, 0);
% Ground Displacement
\draw[dashed] (-1, 0) -- ++(-0.5, 0) coordinate(w);
\draw[->] (w) -- ++(0, 0.5) node[left]{$w$};
% Mass
\draw[fill=white] (-1, 1.4) rectangle ++(2, 0.8) node[pos=0.5]{$m$};
\node[forcesensor={0.4}{0.4}] (fsensn) at (0, 1){};
\draw[] (-0.8, 1) -- (0.8, 1);
\node[left] at (fsensn.west) {$F_m$};
% Displacement of the mass
\draw[dashed] (-1, 2.2) -- ++(-0.5, 0) coordinate(x);
\draw[->] (x) -- ++(0, 0.5) node[left]{$x$};
% Spring, Damper, and Actuator
\draw[spring] (-0.8, 0) -- (-0.8, 1) node[midway, left=0.1]{$k$};
\draw[damper] (0, 0) -- (0, 1) node[midway, left=0.2]{$c$};
\draw[actuator={0.4}{0.2}] (0.8, 0) -- (0.8, 1) coordinate[midway, right=0.1](F);
% Displacements
\node[block={0.8cm}{0.6cm}, right=0.6 of F] (Kiff) {$K$};
\draw[->] (Kiff.west) -- (F) node[above right]{$F$};
\draw[<-] (Kiff.east) -- ++(0.5, 0) |- (fsensn.east);
\end{tikzpicture}
#+end_src
#+name: fig:iff_1dof
#+caption: Integral Force Feedback applied to a 1dof system
#+RESULTS:
[[file:figs/iff_1dof.png]]
The dynamic of the system is described by the following equation:
\begin{equation}
ms^2x = F_d - kx - csx + kw + csw + F
\end{equation}
The measured force $F_m$ is:
\begin{align}
F_m &= F - kx - csx + kw + csw \\
&= ms^2 x - F_d
\end{align}
The Integral Force Feedback controller is $K = -\frac{g}{s}$, and thus the applied force by this controller is:
\begin{equation}
F_{\text{IFF}} = -\frac{g}{s} F_m = -\frac{g}{s} (ms^2 x - F_d)
\end{equation}
Once the IFF is applied, the new dynamics of the system is:
\begin{equation}
ms^2x = F_d + F - kx - csx + kw + csw - \frac{g}{s} (ms^2x - F_d)
\end{equation}
And finally:
\begin{equation}
x = F_d \frac{1 + \frac{g}{s}}{ms^2 + (mg + c)s + k} + F \frac{1}{ms^2 + (mg + c)s + k} + w \frac{k + cs}{ms^2 + (mg + c)s + k}
\end{equation}
We can see that this:
- adds damping to the system by a value $mg$
- lower the compliance as low frequency by a factor: $1 + g/s$
If we want critical damping:
\begin{equation}
\xi = \frac{1}{2} \frac{c + gm}{\sqrt{km}} = \frac{1}{2}
\end{equation}
This is attainable if we have:
\begin{equation}
g = \frac{\sqrt{km} - c}{m}
\end{equation}
*** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
*** Matlab Example
Let define the system parameters.
#+begin_src matlab
m = 50; % [kg]
k = 1e6; % [N/m]
c = 1e3; % [N/(m/s)]
#+end_src
The state space model of the system is defined below.
#+begin_src matlab
A = [-c/m -k/m;
1 0];
B = [1/m 1/m -1;
0 0 0];
C = [ 0 1;
-c -k];
D = [0 0 0;
1 0 0];
sys = ss(A, B, C, D);
sys.InputName = {'F', 'Fd', 'wddot'};
sys.OutputName = {'d', 'Fm'};
sys.StateName = {'ddot', 'd'};
#+end_src
The controller $K_\text{IFF}$ is:
#+begin_src matlab
Kiff = -((sqrt(k*m)-c)/m)/s;
Kiff.InputName = {'Fm'};
Kiff.OutputName = {'F'};
#+end_src
And the closed loop system is computed below.
#+begin_src matlab
sys_iff = feedback(sys, Kiff, 'name', +1);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
subplot(2, 2, 1);
title('Fd to d')
hold on;
plot(freqs, abs(squeeze(freqresp(sys('d', 'Fd'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(sys_iff('d', 'Fd'), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
xlim([freqs(1), freqs(end)]);
subplot(2, 2, 3);
title('Fd to x')
hold on;
plot(freqs, abs(squeeze(freqresp(sys('d', 'Fd'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(sys_iff('d', 'Fd'), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
xlim([freqs(1), freqs(end)]);
subplot(2, 2, 2);
title('w to d')
hold on;
plot(freqs, abs(squeeze(freqresp(sys('d', 'wddot')*s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(sys_iff('d', 'wddot')*s^2, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
xlim([freqs(1), freqs(end)]);
subplot(2, 2, 4);
title('w to x')
hold on;
plot(freqs, abs(squeeze(freqresp(1+sys('d', 'wddot')*s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(1+sys_iff('d', 'wddot')*s^2, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
xlim([freqs(1), freqs(end)]);
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/iff_1dof_sensitivitiy.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:iff_1dof_sensitivitiy
#+CAPTION: Sensitivity to disturbance when IFF is applied on the 1dof system ([[./figs/iff_1dof_sensitivitiy.png][png]], [[./figs/iff_1dof_sensitivitiy.pdf][pdf]])
[[file:figs/iff_1dof_sensitivitiy.png]]
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
@ -566,8 +750,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0. All the controllers are set to 0.
@ -1106,8 +1290,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
And initialize the controllers. And initialize the controllers.
@ -1617,8 +1801,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
And initialize the controllers. And initialize the controllers.

42
active_damping_uniaxial/index.org
View File

@ -1,4 +1,4 @@
#+TITLE: Active Damping #+TITLE: Active Damping with an uni-axial model
:DRAWER: :DRAWER:
#+STARTUP: overview #+STARTUP: overview
@ -97,7 +97,7 @@ The performance of this undamped system will be compared with the damped system
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
open('active_damping/matlab/sim_nano_station_id.slx') open('active_damping_uniaxial/matlab/sim_nano_station_id.slx')
#+end_src #+end_src
** Init ** Init
@ -114,8 +114,8 @@ The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0. All the controllers are set to 0.
@ -138,7 +138,7 @@ We identify the various transfer functions of the system
And we save it for further analysis. And we save it for further analysis.
#+begin_src matlab #+begin_src matlab
save('./active_damping/mat/plants.mat', 'G', '-append'); save('./active_damping_uniaxial/mat/plants.mat', 'G', '-append');
#+end_src #+end_src
** Sensitivity to disturbances ** Sensitivity to disturbances
@ -462,13 +462,13 @@ And the closed loop system is computed below.
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
open('active_damping/matlab/sim_nano_station_id.slx') open('active_damping_uniaxial/matlab/sim_nano_station_id.slx')
#+end_src #+end_src
** Control Design ** Control Design
Let's load the undamped plant: Let's load the undamped plant:
#+begin_src matlab #+begin_src matlab
load('./active_damping/mat/plants.mat', 'G'); load('./active_damping_uniaxial/mat/plants.mat', 'G');
#+end_src #+end_src
Let's look at the transfer function from actuator forces in the nano-hexapod to the force sensor in the nano-hexapod legs for all 6 pairs of actuator/sensor (figure [[fig:iff_plant]]). Let's look at the transfer function from actuator forces in the nano-hexapod to the force sensor in the nano-hexapod legs for all 6 pairs of actuator/sensor (figure [[fig:iff_plant]]).
@ -566,8 +566,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0. All the controllers are set to 0.
@ -589,7 +589,7 @@ We identify the system dynamics now that the IFF controller is ON.
And we save the damped plant for further analysis And we save the damped plant for further analysis
#+begin_src matlab #+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_iff', '-append'); save('./active_damping_uniaxial/mat/plants.mat', 'G_iff', '-append');
#+end_src #+end_src
** Sensitivity to disturbances ** Sensitivity to disturbances
@ -1001,13 +1001,13 @@ And the closed loop system is computed below.
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
open('active_damping/matlab/sim_nano_station_id.slx') open('active_damping_uniaxial/matlab/sim_nano_station_id.slx')
#+end_src #+end_src
** Control Design ** Control Design
Let's load the undamped plant: Let's load the undamped plant:
#+begin_src matlab #+begin_src matlab
load('./active_damping/mat/plants.mat', 'G'); load('./active_damping_uniaxial/mat/plants.mat', 'G');
#+end_src #+end_src
Let's look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure [[fig:rmc_plant]]). Let's look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure [[fig:rmc_plant]]).
@ -1106,8 +1106,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
And initialize the controllers. And initialize the controllers.
@ -1129,7 +1129,7 @@ We identify the system dynamics now that the RMC controller is ON.
And we save the damped plant for further analysis. And we save the damped plant for further analysis.
#+begin_src matlab #+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_rmc', '-append'); save('./active_damping_uniaxial/mat/plants.mat', 'G_rmc', '-append');
#+end_src #+end_src
** Sensitivity to disturbances ** Sensitivity to disturbances
@ -1514,13 +1514,13 @@ The obtained sensitivity to disturbances is shown in figure [[fig:dvf_1dof_sensi
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
open('active_damping/matlab/sim_nano_station_id.slx') open('active_damping_uniaxial/matlab/sim_nano_station_id.slx')
#+end_src #+end_src
** Control Design ** Control Design
Let's load the undamped plant: Let's load the undamped plant:
#+begin_src matlab #+begin_src matlab
load('./active_damping/mat/plants.mat', 'G'); load('./active_damping_uniaxial/mat/plants.mat', 'G');
#+end_src #+end_src
Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure [[fig:dvf_plant]]). Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure [[fig:dvf_plant]]).
@ -1617,8 +1617,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
And initialize the controllers. And initialize the controllers.
@ -1640,7 +1640,7 @@ We identify the system dynamics now that the RMC controller is ON.
And we save the damped plant for further analysis. And we save the damped plant for further analysis.
#+begin_src matlab #+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_dvf', '-append'); save('./active_damping_uniaxial/mat/plants.mat', 'G_dvf', '-append');
#+end_src #+end_src
** Sensitivity to disturbances ** Sensitivity to disturbances
@ -1812,7 +1812,7 @@ Direct Velocity Feedback:
** Load the plants ** Load the plants
#+begin_src matlab #+begin_src matlab
load('./active_damping/mat/plants.mat', 'G', 'G_iff', 'G_rmc', 'G_dvf'); load('./active_damping_uniaxial/mat/plants.mat', 'G', 'G_iff', 'G_rmc', 'G_dvf');
#+end_src #+end_src
** Sensitivity to Disturbance ** Sensitivity to Disturbance

4
disturbances/index.org
View File

@ -217,8 +217,8 @@ We initialize all the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
* Identification * Identification

28
experiment_tomography/index.org
View File

@ -93,14 +93,14 @@ We first initialize all the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 1)); initializeSample('mass', 1);
#+end_src #+end_src
We initialize the reference path for all the stages. We initialize the reference path for all the stages.
All stage is set to its zero position except the Spindle which is rotating at 60rpm. All stage is set to its zero position except the Spindle which is rotating at 60rpm.
#+begin_src matlab #+begin_src matlab
initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1)); initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
#+end_src #+end_src
* Tomography Experiment with no disturbances * Tomography Experiment with no disturbances
@ -110,7 +110,7 @@ All stage is set to its zero position except the Spindle which is rotating at 60
** Simulation Setup ** Simulation Setup
And we initialize the disturbances to be equal to zero. And we initialize the disturbances to be equal to zero.
#+begin_src matlab #+begin_src matlab
opts = struct(... initDisturbances(...
'Dwx', false, ... % Ground Motion - X direction 'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction 'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction 'Dwz', false, ... % Ground Motion - Z direction
@ -118,7 +118,6 @@ And we initialize the disturbances to be equal to zero.
'Fty_z', false, ... % Translation Stage - Z direction 'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction 'Frz_z', false ... % Spindle - Z direction
); );
initDisturbances(opts);
#+end_src #+end_src
We simulate the model. We simulate the model.
@ -221,7 +220,7 @@ And we save the obtained data.
** Simulation Setup ** Simulation Setup
We now activate the disturbances. We now activate the disturbances.
#+begin_src matlab #+begin_src matlab
opts = struct(... initDisturbances(...
'Dwx', true, ... % Ground Motion - X direction 'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction 'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction 'Dwz', true, ... % Ground Motion - Z direction
@ -229,7 +228,6 @@ We now activate the disturbances.
'Fty_z', true, ... % Translation Stage - Z direction 'Fty_z', true, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction 'Frz_z', true ... % Spindle - Z direction
); );
initDisturbances(opts);
#+end_src #+end_src
We simulate the model. We simulate the model.
@ -336,17 +334,17 @@ We first set the wanted translation of the Micro Hexapod.
We initialize the reference path. We initialize the reference path.
#+begin_src matlab #+begin_src matlab
initializeReferences(struct('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1)); initializeReferences('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1);
#+end_src #+end_src
We initialize the stages. We initialize the stages.
#+begin_src matlab #+begin_src matlab
initializeMicroHexapod(struct('AP', P_micro_hexapod)); initializeMicroHexapod('AP', P_micro_hexapod);
#+end_src #+end_src
And we initialize the disturbances to zero. And we initialize the disturbances to zero.
#+begin_src matlab #+begin_src matlab
opts = struct(... initDisturbances(...
'Dwx', false, ... % Ground Motion - X direction 'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction 'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction 'Dwz', false, ... % Ground Motion - Z direction
@ -354,7 +352,6 @@ And we initialize the disturbances to zero.
'Fty_z', false, ... % Translation Stage - Z direction 'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction 'Frz_z', false ... % Spindle - Z direction
); );
initDisturbances(opts);
#+end_src #+end_src
We simulate the model. We simulate the model.
@ -456,7 +453,7 @@ And we save the obtained data.
** Simulation Setup ** Simulation Setup
We set the reference path. We set the reference path.
#+begin_src matlab #+begin_src matlab
initializeReferences(struct('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1)); initializeReferences('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1);
#+end_src #+end_src
We initialize the stages. We initialize the stages.
@ -469,13 +466,13 @@ We initialize the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 1)); initializeSample('mass', 1);
#+end_src #+end_src
And we initialize the disturbances to zero. And we initialize the disturbances to zero.
#+begin_src matlab #+begin_src matlab
opts = struct(... initDisturbances(...
'Dwx', false, ... % Ground Motion - X direction 'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction 'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction 'Dwz', false, ... % Ground Motion - Z direction
@ -483,7 +480,6 @@ And we initialize the disturbances to zero.
'Fty_z', false, ... % Translation Stage - Z direction 'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction 'Frz_z', false ... % Spindle - Z direction
); );
initDisturbances(opts);
#+end_src #+end_src
We simulate the model. We simulate the model.

12
identification/index.org
View File

@ -88,8 +88,8 @@ We initialize all the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
** Compute the transfer functions ** Compute the transfer functions
@ -173,8 +173,8 @@ We initialize all the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
** Identification ** Identification
@ -297,8 +297,8 @@ We initialize all the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
** Estimate the position of the CoM of each solid and compare with the one took for the Measurement Analysis ** Estimate the position of the CoM of each solid and compare with the one took for the Measurement Analysis

3
kinematics/index.org
View File

@ -177,7 +177,7 @@ We define the wanted position/orientation of the Hexapod under study.
ARB = Rz*Ry*Rx; ARB = Rz*Ry*Rx;
AP = [0.01; 0.02; 0.03]; % [m] AP = [0.01; 0.02; 0.03]; % [m]
hexapod = initializeMicroHexapod(struct('AP', AP, 'ARB', ARB)); hexapod = initializeMicroHexapod('AP', AP, 'ARB', ARB);
#+end_src #+end_src
We run the simulation. We run the simulation.
@ -203,6 +203,7 @@ And we verify that we indeed succeed to go to the wanted position.
| -1.2659e-10 | 6.5603e-11 | 6.2183e-10 | | -1.2659e-10 | 6.5603e-11 | 6.2183e-10 |
| 1.0354e-10 | -5.2439e-11 | -5.2425e-10 | | 1.0354e-10 | -5.2439e-11 | -5.2425e-10 |
| -5.9816e-10 | 5.532e-10 | -1.7737e-10 | | -5.9816e-10 | 5.532e-10 | -1.7737e-10 |
* TODO Tests on the transformation from reference to wanted position :noexport: * TODO Tests on the transformation from reference to wanted position :noexport:
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :eval no :header-args:matlab+: :eval no

34
positioning_error/index.org
View File

@ -102,13 +102,13 @@ We initialize all the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
We setup the reference path to be constant. We setup the reference path to be constant.
#+begin_src matlab #+begin_src matlab
opts = struct( ... initializeReferences(...
'Ts', 1e-3, ... % Sampling Frequency [s] 'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" 'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 5e-3, ... % Amplitude of the displacement [m] 'Dy_amplitude', 5e-3, ... % Amplitude of the displacement [m]
@ -126,9 +126,6 @@ We setup the reference path to be constant.
'Dn_type', 'constant', ... % For now, only constant is implemented 'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [1e-3; 2e-3; 3e-3; 1*pi/180; 0; 1*pi/180] ... % Initial position [m,m,m,rad,rad,rad] of the top platform 'Dn_pos', [1e-3; 2e-3; 3e-3; 1*pi/180; 0; 1*pi/180] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
); );
initializeReferences(opts);
#+end_src #+end_src
No position error for now (perfect positioning). No position error for now (perfect positioning).
@ -253,13 +250,13 @@ We initialize all the stages.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
We setup the reference path to be constant. We setup the reference path to be constant.
#+begin_src matlab #+begin_src matlab
opts = struct( ... initializeReferences(...
'Ts', 1e-3, ... % Sampling Frequency [s] 'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" 'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 0, ... % Amplitude of the displacement [m] 'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
@ -274,8 +271,6 @@ We setup the reference path to be constant.
'Dn_type', 'constant', ... % For now, only constant is implemented 'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform 'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
); );
initializeReferences(opts);
#+end_src #+end_src
Now we introduce some positioning error. Now we introduce some positioning error.
@ -383,8 +378,21 @@ Verify that the pose error corresponds to the positioning error of the stages.
** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end ** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error. We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
#+begin_src matlab #+begin_src matlab
opts.Dn_pos = [Edx, Edy, Edz, Erx, Ery, Erz]'; initializeReferences(...
initializeReferences(opts); 'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [rad]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0*pi/180, ... % Initial angle [rad]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0, pi]', ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [Edx, Edy, Edz, Erx, Ery, Erz]' ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
#+end_src #+end_src
And we run the simulation. And we run the simulation.

361
simscape_subsystems/index.org
View File

@ -53,60 +53,81 @@
This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
*** Function Declaration and Documentation
#+begin_src matlab #+begin_src matlab
function [ref] = initializeReferences(opts_param) function [ref] = initializeReferences(args)
%% Default values for opts #+end_src
opts = struct( ...
'Ts', 1e-3, ... % Sampling Frequency [s]
'Tmax', 100, ... % Maximum simulation time [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Dy_period', 1, ... % Period of the displacement [s]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [rad]
'Ry_period', 1, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', zeros(6, 1), ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0; pi], ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', zeros(6,1) ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
%% Populate opts with input parameters *** Optional Parameters
if exist('opts_param','var') #+begin_src matlab
for opt = fieldnames(opts_param)' arguments
opts.(opt{1}) = opts_param.(opt{1}); % Sampling Frequency [s]
end args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
% Maximum simulation time [s]
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
% Either "constant" / "triangular" / "sinusoidal"
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude of the displacement [m]
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "triangular" / "sinusoidal"
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude [rad]
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "rotating"
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating'})} = 'constant'
% Initial angle [rad]
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the rotating [s]
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% For now, only constant is implemented
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
% For now, only constant is implemented
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
% Initial position of the two masses
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
% For now, only constant is implemented
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
end end
#+end_src
*** Initialize Parameters
#+begin_src matlab
%% Set Sampling Time %% Set Sampling Time
Ts = opts.Ts; Ts = args.Ts;
Tmax = opts.Tmax; Tmax = args.Tmax;
%% Low Pass Filter to filter out the references %% Low Pass Filter to filter out the references
s = zpk('s'); s = zpk('s');
w0 = 2*pi*100; w0 = 2*pi*100;
xi = 1; xi = 1;
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2); H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
#+end_src
*** Translation Stage
#+begin_src matlab
%% Translation stage - Dy %% Translation stage - Dy
t = 0:Ts:Tmax; % Time Vector [s] t = 0:Ts:Tmax; % Time Vector [s]
Dy = zeros(length(t), 1); Dy = zeros(length(t), 1);
Dyd = zeros(length(t), 1); Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1); Dydd = zeros(length(t), 1);
switch opts.Dy_type switch args.Dy_type
case 'constant' case 'constant'
Dy(:) = opts.Dy_amplitude; Dy(:) = args.Dy_amplitude;
Dyd(:) = 0; Dyd(:) = 0;
Dydd(:) = 0; Dydd(:) = 0;
case 'triangular' case 'triangular'
% This is done to unsure that we start with no displacement % This is done to unsure that we start with no displacement
Dy_raw = opts.Dy_amplitude*sawtooth(2*pi*t/opts.Dy_period,1/2); Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
i0 = find(t>=opts.Dy_period/4,1); i0 = find(t>=args.Dy_period/4,1);
Dy(1:end-i0+1) = Dy_raw(i0:end); Dy(1:end-i0+1) = Dy_raw(i0:end);
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
@ -115,29 +136,32 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Dyd = lsim(H_lpf*s, Dy, t); Dyd = lsim(H_lpf*s, Dy, t);
Dydd = lsim(H_lpf*s^2, Dy, t); Dydd = lsim(H_lpf*s^2, Dy, t);
case 'sinusoidal' case 'sinusoidal'
Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t); Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
Dyd = opts.Dy_amplitude*2*pi/opts.Dy_period*cos(2*pi/opts.Dy_period*t); Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
Dydd = -opts.Dy_amplitude*(2*pi/opts.Dy_period)^2*sin(2*pi/opts.Dy_period*t); Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
otherwise otherwise
warning('Dy_type is not set correctly'); warning('Dy_type is not set correctly');
end end
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd); Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
#+end_src
*** Tilt Stage
#+begin_src matlab
%% Tilt Stage - Ry %% Tilt Stage - Ry
t = 0:Ts:Tmax; % Time Vector [s] t = 0:Ts:Tmax; % Time Vector [s]
Ry = zeros(length(t), 1); Ry = zeros(length(t), 1);
Ryd = zeros(length(t), 1); Ryd = zeros(length(t), 1);
Rydd = zeros(length(t), 1); Rydd = zeros(length(t), 1);
switch opts.Ry_type switch args.Ry_type
case 'constant' case 'constant'
Ry(:) = opts.Ry_amplitude; Ry(:) = args.Ry_amplitude;
Ryd(:) = 0; Ryd(:) = 0;
Rydd(:) = 0; Rydd(:) = 0;
case 'triangular' case 'triangular'
Ry_raw = opts.Ry_amplitude*sawtooth(2*pi*t/opts.Ry_period,1/2); Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
i0 = find(t>=opts.Ry_period/4,1); i0 = find(t>=args.Ry_period/4,1);
Ry(1:end-i0+1) = Ry_raw(i0:end); Ry(1:end-i0+1) = Ry_raw(i0:end);
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
@ -146,29 +170,32 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Ryd = lsim(H_lpf*s, Ry, t); Ryd = lsim(H_lpf*s, Ry, t);
Rydd = lsim(H_lpf*s^2, Ry, t); Rydd = lsim(H_lpf*s^2, Ry, t);
case 'sinusoidal' case 'sinusoidal'
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t); Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
Ryd = opts.Ry_amplitude*2*pi/opts.Ry_period*cos(2*pi/opts.Ry_period*t); Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
Rydd = -opts.Ry_amplitude*(2*pi/opts.Ry_period)^2*sin(2*pi/opts.Ry_period*t); Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
otherwise otherwise
warning('Ry_type is not set correctly'); warning('Ry_type is not set correctly');
end end
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd); Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
#+end_src
*** Spindle
#+begin_src matlab
%% Spindle - Rz %% Spindle - Rz
t = 0:Ts:Tmax; % Time Vector [s] t = 0:Ts:Tmax; % Time Vector [s]
Rz = zeros(length(t), 1); Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1); Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1); Rzdd = zeros(length(t), 1);
switch opts.Rz_type switch args.Rz_type
case 'constant' case 'constant'
Rz(:) = opts.Rz_amplitude; Rz(:) = args.Rz_amplitude;
Rzd(:) = 0; Rzd(:) = 0;
Rzdd(:) = 0; Rzdd(:) = 0;
case 'rotating' case 'rotating'
Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t; Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t;
% The signal is filtered out % The signal is filtered out
Rz = lsim(H_lpf, Rz, t); Rz = lsim(H_lpf, Rz, t);
@ -179,23 +206,26 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
end end
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd); Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
#+end_src
*** Micro Hexapod
#+begin_src matlab
%% Micro-Hexapod %% Micro-Hexapod
t = [0, Ts]; t = [0, Ts];
Dh = zeros(length(t), 6); Dh = zeros(length(t), 6);
Dhl = zeros(length(t), 6); Dhl = zeros(length(t), 6);
switch opts.Dh_type switch args.Dh_type
case 'constant' case 'constant'
Dh = [opts.Dh_pos, opts.Dh_pos]; Dh = [args.Dh_pos, args.Dh_pos];
load('./mat/stages.mat', 'micro_hexapod'); load('mat/stages.mat', 'micro_hexapod');
AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)]; AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
tx = opts.Dh_pos(4); tx = args.Dh_pos(4);
ty = opts.Dh_pos(5); ty = args.Dh_pos(5);
tz = opts.Dh_pos(6); tz = args.Dh_pos(6);
ARB = [cos(tz) -sin(tz) 0; ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0; sin(tz) cos(tz) 0;
@ -215,28 +245,37 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Dh = struct('time', t, 'signals', struct('values', Dh)); Dh = struct('time', t, 'signals', struct('values', Dh));
Dhl = struct('time', t, 'signals', struct('values', Dhl)); Dhl = struct('time', t, 'signals', struct('values', Dhl));
#+end_src
*** Axis Compensation
#+begin_src matlab
%% Axis Compensation - Rm %% Axis Compensation - Rm
t = [0, Ts]; t = [0, Ts];
Rm = [opts.Rm_pos, opts.Rm_pos]; Rm = [args.Rm_pos, args.Rm_pos];
Rm = struct('time', t, 'signals', struct('values', Rm)); Rm = struct('time', t, 'signals', struct('values', Rm));
#+end_src
*** Nano Hexapod
#+begin_src matlab
%% Nano-Hexapod %% Nano-Hexapod
t = [0, Ts]; t = [0, Ts];
Dn = zeros(length(t), 6); Dn = zeros(length(t), 6);
switch opts.Dn_type switch args.Dn_type
case 'constant' case 'constant'
Dn = [opts.Dn_pos, opts.Dn_pos]; Dn = [args.Dn_pos, args.Dn_pos];
otherwise otherwise
warning('Dn_type is not set correctly'); warning('Dn_type is not set correctly');
end end
Dn = struct('time', t, 'signals', struct('values', Dn)); Dn = struct('time', t, 'signals', struct('values', Dn));
#+end_src
*** Save
#+begin_src matlab
%% Save %% Save
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts'); save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
end end
#+end_src #+end_src
@ -252,36 +291,35 @@ This Matlab function is accessible [[file:src/initDisturbances.m][here]].
*** Function Declaration and Documentation *** Function Declaration and Documentation
#+begin_src matlab #+begin_src matlab
function [] = initDisturbances(opts_param) function [] = initDisturbances(args)
% initDisturbances - Initialize the disturbances % initDisturbances - Initialize the disturbances
% %
% Syntax: [] = initDisturbances(opts_param) % Syntax: [] = initDisturbances(args)
% %
% Inputs: % Inputs:
% - opts_param - % - args -
#+end_src #+end_src
*** Default values for the Options *** Optional Parameters
#+begin_src matlab #+begin_src matlab
%% Default values for opts arguments
opts = struct(... % Ground Motion - X direction
'Dwx', true, ... % Ground Motion - X direction args.Dwx logical {mustBeNumericOrLogical} = true
'Dwy', true, ... % Ground Motion - Y direction % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction args.Dwy logical {mustBeNumericOrLogical} = true
'Fty_x', true, ... % Translation Stage - X direction % Ground Motion - Z direction
'Fty_z', true, ... % Translation Stage - Z direction args.Dwz logical {mustBeNumericOrLogical} = true
'Frz_z', true ... % Spindle - Z direction % Translation Stage - X direction
); args.Fty_x logical {mustBeNumericOrLogical} = true
% Translation Stage - Z direction
%% Populate opts with input parameters args.Fty_z logical {mustBeNumericOrLogical} = true
if exist('opts_param','var') % Spindle - Z direction
for opt = fieldnames(opts_param)' args.Frz_z logical {mustBeNumericOrLogical} = true
opts.(opt{1}) = opts_param.(opt{1});
end
end end
#+end_src #+end_src
*** Load Data *** Load Data
#+begin_src matlab #+begin_src matlab
load('./disturbances/mat/dist_psd.mat', 'dist_f'); load('./disturbances/mat/dist_psd.mat', 'dist_f');
@ -317,7 +355,7 @@ We define some parameters that will be used in the algorithm.
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
if opts.Dwx if args.Dwx
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
@ -328,7 +366,7 @@ We define some parameters that will be used in the algorithm.
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
if opts.Dwy if args.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
@ -339,7 +377,7 @@ We define some parameters that will be used in the algorithm.
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
if opts.Dwy if args.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
@ -351,7 +389,7 @@ We define some parameters that will be used in the algorithm.
*** Translation Stage - X direction *** Translation Stage - X direction
#+begin_src matlab #+begin_src matlab
if opts.Fty_x if args.Fty_x
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -369,7 +407,7 @@ We define some parameters that will be used in the algorithm.
*** Translation Stage - Z direction *** Translation Stage - Z direction
#+begin_src matlab #+begin_src matlab
if opts.Fty_z if args.Fty_z
phi = dist_f.psd_ty; phi = dist_f.psd_ty;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -387,7 +425,7 @@ We define some parameters that will be used in the algorithm.
*** Spindle - Z direction *** Spindle - Z direction
#+begin_src matlab #+begin_src matlab
if opts.Frz_z if args.Frz_z
phi = dist_f.psd_rz; phi = dist_f.psd_rz;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -421,7 +459,7 @@ We define some parameters that will be used in the algorithm.
*** Save *** Save
#+begin_src matlab #+begin_src matlab
save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't'); save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
#+end_src #+end_src
* Initialize Elements * Initialize Elements
@ -462,15 +500,9 @@ end
This Matlab function is accessible [[file:../src/initializeGranite.m][here]]. This Matlab function is accessible [[file:../src/initializeGranite.m][here]].
#+begin_src matlab #+begin_src matlab
function [granite] = initializeGranite(opts_param) function [granite] = initializeGranite(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -486,7 +518,7 @@ This Matlab function is accessible [[file:../src/initializeGranite.m][here]].
granite.mass_top = 4000; % [kg] TODO granite.mass_top = 4000; % [kg] TODO
%% Dynamical Properties %% Dynamical Properties
if opts.rigid if args.rigid
granite.k.x = 1e12; % [N/m] granite.k.x = 1e12; % [N/m]
granite.k.y = 1e12; % [N/m] granite.k.y = 1e12; % [N/m]
granite.k.z = 1e12; % [N/m] granite.k.z = 1e12; % [N/m]
@ -527,15 +559,9 @@ This Matlab function is accessible [[file:../src/initializeGranite.m][here]].
This Matlab function is accessible [[file:../src/initializeTy.m][here]]. This Matlab function is accessible [[file:../src/initializeTy.m][here]].
#+begin_src matlab #+begin_src matlab
function [ty] = initializeTy(opts_param) function [ty] = initializeTy(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -582,7 +608,7 @@ This Matlab function is accessible [[file:../src/initializeTy.m][here]].
ty.m = 1000; % TODO [kg] ty.m = 1000; % TODO [kg]
%% Y-Translation - Dynamicals Properties %% Y-Translation - Dynamicals Properties
if opts.rigid if args.rigid
ty.k.ax = 1e12; % Axial Stiffness for each of the 4 guidance (y) [N/m] ty.k.ax = 1e12; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad = 1e12; % Radial Stiffness for each of the 4 guidance (x-z) [N/m] ty.k.rad = 1e12; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
else else
@ -608,15 +634,9 @@ This Matlab function is accessible [[file:../src/initializeTy.m][here]].
This Matlab function is accessible [[file:../src/initializeRy.m][here]]. This Matlab function is accessible [[file:../src/initializeRy.m][here]].
#+begin_src matlab #+begin_src matlab
function [ry] = initializeRy(opts_param) function [ry] = initializeRy(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -643,7 +663,7 @@ This Matlab function is accessible [[file:../src/initializeRy.m][here]].
ry.m = 800; % TODO [kg] ry.m = 800; % TODO [kg]
%% Tilt Stage - Dynamical Properties %% Tilt Stage - Dynamical Properties
if opts.rigid if args.rigid
ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg] ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg]
ry.k.h = 1e12; % Stiffness in the direction of the guidance [N/m] ry.k.h = 1e12; % Stiffness in the direction of the guidance [N/m]
ry.k.rad = 1e12; % Stiffness in the top direction [N/m] ry.k.rad = 1e12; % Stiffness in the top direction [N/m]
@ -678,15 +698,9 @@ This Matlab function is accessible [[file:../src/initializeRy.m][here]].
This Matlab function is accessible [[file:../src/initializeRz.m][here]]. This Matlab function is accessible [[file:../src/initializeRz.m][here]].
#+begin_src matlab #+begin_src matlab
function [rz] = initializeRz(opts_param) function [rz] = initializeRz(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -711,7 +725,7 @@ This Matlab function is accessible [[file:../src/initializeRz.m][here]].
%% Spindle - Dynamical Properties %% Spindle - Dynamical Properties
if opts.rigid if args.rigid
rz.k.rot = 1e10; % Rotational Stiffness (Rz) [N*m/deg] rz.k.rot = 1e10; % Rotational Stiffness (Rz) [N*m/deg]
rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg] rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg]
rz.k.ax = 1e12; % Axial Stiffness (Z) [N/m] rz.k.ax = 1e12; % Axial Stiffness (Z) [N/m]
@ -744,19 +758,11 @@ This Matlab function is accessible [[file:../src/initializeRz.m][here]].
This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]]. This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]].
#+begin_src matlab #+begin_src matlab
function [micro_hexapod] = initializeMicroHexapod(opts_param) function [micro_hexapod] = initializeMicroHexapod(args)
%% Default values for opts arguments
opts = struct(... args.rigid logical {mustBeNumericOrLogical} = false
'rigid', false, ... args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} args.ARB (3,3) double {mustBeNumeric} = eye(3)
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% Stewart Object %% Stewart Object
@ -792,7 +798,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
Leg = struct(); Leg = struct();
Leg.stroke = 10e-3; % Maximum Stroke of each leg [m] Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
if opts.rigid if args.rigid
Leg.k.ax = 1e12; % Stiffness of each leg [N/m] Leg.k.ax = 1e12; % Stiffness of each leg [N/m]
else else
Leg.k.ax = 2e7; % Stiffness of each leg [N/m] Leg.k.ax = 2e7; % Stiffness of each leg [N/m]
@ -844,7 +850,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
micro_hexapod = initializeParameters(micro_hexapod); micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg %% Setup equilibrium position of each leg
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB); micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);
%% Save %% Save
save('./mat/stages.mat', 'micro_hexapod', '-append'); save('./mat/stages.mat', 'micro_hexapod', '-append');
@ -1003,18 +1009,10 @@ This Matlab function is accessible [[file:../src/initializeAxisc.m][here]].
This Matlab function is accessible [[file:../src/initializeMirror.m][here]]. This Matlab function is accessible [[file:../src/initializeMirror.m][here]].
#+begin_src matlab #+begin_src matlab
function [] = initializeMirror(opts_param) function [] = initializeMirror(args)
%% Default values for opts arguments
opts = struct(... args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
'shape', 'spherical', ... % spherical or conical args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45
'angle', 45 ...
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -1029,7 +1027,7 @@ This Matlab function is accessible [[file:../src/initializeMirror.m][here]].
mirror.density = 2400; % Density of the mirror [kg/m3] mirror.density = 2400; % Density of the mirror [kg/m3]
mirror.color = [0.4 1.0 1.0]; % Color of the mirror mirror.color = [0.4 1.0 1.0]; % Color of the mirror
mirror.cone_length = mirror.rad*tand(opts.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
%% Shape %% Shape
mirror.shape = [... mirror.shape = [...
@ -1039,14 +1037,14 @@ This Matlab function is accessible [[file:../src/initializeMirror.m][here]].
mirror.rad 0 ... mirror.rad 0 ...
]; ];
if strcmp(opts.shape, 'spherical') if strcmp(args.shape, 'spherical')
mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm] mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]
for z = linspace(0, mirror.h, 101) for z = linspace(0, mirror.h, 101)
mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z]; mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
end end
elseif strcmp(opts.shape, 'conical') elseif strcmp(args.shape, 'conical')
mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.angle) mirror.h]; mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h];
else else
error('Shape should be either conical or spherical'); error('Shape should be either conical or spherical');
end end
@ -1068,22 +1066,17 @@ This Matlab function is accessible [[file:../src/initializeMirror.m][here]].
This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]]. This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]].
#+begin_src matlab #+begin_src matlab
function [nano_hexapod] = initializeNanoHexapod(opts_param) function [nano_hexapod] = initializeNanoHexapod(args)
%% Default values for opts arguments
opts = struct('actuator', 'piezo'); args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
%% Populate opts with input parameters args.ARB (3,3) double {mustBeNumeric} = eye(3)
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% Stewart Object %% Stewart Object
nano_hexapod = struct(); nano_hexapod = struct();
nano_hexapod.h = 90; % Total height of the platform [mm] nano_hexapod.h = 90; % Total height of the platform [mm]
nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm] nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm]
% nano_hexapod.jacobian = 174.26; % Point where the Jacobian is computed => Center of rotation [mm]
%% Bottom Plate %% Bottom Plate
BP = struct(); BP = struct();
@ -1113,12 +1106,12 @@ This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]]
Leg = struct(); Leg = struct();
Leg.stroke = 80e-6; % Maximum Stroke of each leg [m] Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
if strcmp(opts.actuator, 'piezo') if strcmp(args.actuator, 'piezo')
Leg.k.ax = 1e7; % Stiffness of each leg [N/m] Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
elseif strcmp(opts.actuator, 'lorentz') elseif strcmp(args.actuator, 'lorentz')
Leg.k.ax = 1e4; % Stiffness of each leg [N/m] Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
else else
error('opts.actuator should be piezo or lorentz'); error('args.actuator should be piezo or lorentz');
end end
Leg.ksi.ax = 10; % Maximum amplification at resonance [] Leg.ksi.ax = 10; % Maximum amplification at resonance []
Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm] Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
@ -1167,6 +1160,9 @@ This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]]
%% %%
nano_hexapod = initializeParameters(nano_hexapod); nano_hexapod = initializeParameters(nano_hexapod);
%% Setup equilibrium position of each leg
nano_hexapod.L0 = inverseKinematicsHexapod(nano_hexapod, args.AP, args.ARB);
%% Save %% Save
save('./mat/stages.mat', 'nano_hexapod', '-append'); save('./mat/stages.mat', 'nano_hexapod', '-append');
@ -1284,17 +1280,7 @@ This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]]
This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]]. This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]].
#+begin_src matlab #+begin_src matlab
function [cedrat] = initializeCedratPiezo(opts_param) function [cedrat] = initializeCedratPiezo()
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Stewart Object %% Stewart Object
cedrat = struct(); cedrat = struct();
cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m] cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]
@ -1323,20 +1309,13 @@ This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]]
This Matlab function is accessible [[file:../src/initializeSample.m][here]]. This Matlab function is accessible [[file:../src/initializeSample.m][here]].
#+begin_src matlab #+begin_src matlab
function [sample] = initializeSample(opts_param) function [sample] = initializeSample(sample)
%% Default values for opts arguments
sample = struct('radius', 100, ... sample.radius (1,1) double {mustBeNumeric, mustBePositive} = 100
'height', 300, ... sample.height (1,1) double {mustBeNumeric, mustBePositive} = 300
'mass', 50, ... sample.mass (1,1) double {mustBeNumeric, mustBePositive} = 50
'offset', 0, ... sample.offset (1,1) double {mustBeNumeric} = 0
'color', [0.45, 0.45, 0.45] ... sample.color (1,3) double {mustBeNumeric} = [0.45, 0.45, 0.45]
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
sample.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%

46
src/initDisturbances.m
View File

@ -1,26 +1,24 @@
function [] = initDisturbances(opts_param) function [] = initDisturbances(args)
% initDisturbances - Initialize the disturbances % initDisturbances - Initialize the disturbances
% %
% Syntax: [] = initDisturbances(opts_param) % Syntax: [] = initDisturbances(args)
% %
% Inputs: % Inputs:
% - opts_param - % - args -
%% Default values for opts arguments
opts = struct(... % Ground Motion - X direction
'Dwx', true, ... % Ground Motion - X direction args.Dwx logical {mustBeNumericOrLogical} = true
'Dwy', true, ... % Ground Motion - Y direction % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction args.Dwy logical {mustBeNumericOrLogical} = true
'Fty_x', true, ... % Translation Stage - X direction % Ground Motion - Z direction
'Fty_z', true, ... % Translation Stage - Z direction args.Dwz logical {mustBeNumericOrLogical} = true
'Frz_z', true ... % Spindle - Z direction % Translation Stage - X direction
); args.Fty_x logical {mustBeNumericOrLogical} = true
% Translation Stage - Z direction
%% Populate opts with input parameters args.Fty_z logical {mustBeNumericOrLogical} = true
if exist('opts_param','var') % Spindle - Z direction
for opt = fieldnames(opts_param)' args.Frz_z logical {mustBeNumericOrLogical} = true
opts.(opt{1}) = opts_param.(opt{1});
end
end end
load('./disturbances/mat/dist_psd.mat', 'dist_f'); load('./disturbances/mat/dist_psd.mat', 'dist_f');
@ -44,7 +42,7 @@ for i = 1:N/2
C(i) = sqrt(phi(i)*df); C(i) = sqrt(phi(i)*df);
end end
if opts.Dwx if args.Dwx
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
@ -53,7 +51,7 @@ else
Dwx = zeros(length(t), 1); Dwx = zeros(length(t), 1);
end end
if opts.Dwy if args.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
@ -62,7 +60,7 @@ else
Dwy = zeros(length(t), 1); Dwy = zeros(length(t), 1);
end end
if opts.Dwy if args.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
@ -71,7 +69,7 @@ else
Dwz = zeros(length(t), 1); Dwz = zeros(length(t), 1);
end end
if opts.Fty_x if args.Fty_x
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -86,7 +84,7 @@ else
Fty_x = zeros(length(t), 1); Fty_x = zeros(length(t), 1);
end end
if opts.Fty_z if args.Fty_z
phi = dist_f.psd_ty; phi = dist_f.psd_ty;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -101,7 +99,7 @@ else
Fty_z = zeros(length(t), 1); Fty_z = zeros(length(t), 1);
end end
if opts.Frz_z if args.Frz_z
phi = dist_f.psd_rz; phi = dist_f.psd_rz;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2

12
src/initializeCedratPiezo.m
View File

@ -1,14 +1,4 @@
function [cedrat] = initializeCedratPiezo(opts_param) function [cedrat] = initializeCedratPiezo()
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Stewart Object %% Stewart Object
cedrat = struct(); cedrat = struct();
cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m] cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]

14
src/initializeGranite.m
View File

@ -1,12 +1,6 @@
function [granite] = initializeGranite(opts_param) function [granite] = initializeGranite(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -22,7 +16,7 @@ function [granite] = initializeGranite(opts_param)
granite.mass_top = 4000; % [kg] TODO granite.mass_top = 4000; % [kg] TODO
%% Dynamical Properties %% Dynamical Properties
if opts.rigid if args.rigid
granite.k.x = 1e12; % [N/m] granite.k.x = 1e12; % [N/m]
granite.k.y = 1e12; % [N/m] granite.k.y = 1e12; % [N/m]
granite.k.z = 1e12; % [N/m] granite.k.z = 1e12; % [N/m]

22
src/initializeMicroHexapod.m
View File

@ -1,16 +1,8 @@
function [micro_hexapod] = initializeMicroHexapod(opts_param) function [micro_hexapod] = initializeMicroHexapod(args)
%% Default values for opts arguments
opts = struct(... args.rigid logical {mustBeNumericOrLogical} = false
'rigid', false, ... args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} args.ARB (3,3) double {mustBeNumeric} = eye(3)
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% Stewart Object %% Stewart Object
@ -46,7 +38,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
Leg = struct(); Leg = struct();
Leg.stroke = 10e-3; % Maximum Stroke of each leg [m] Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
if opts.rigid if args.rigid
Leg.k.ax = 1e12; % Stiffness of each leg [N/m] Leg.k.ax = 1e12; % Stiffness of each leg [N/m]
else else
Leg.k.ax = 2e7; % Stiffness of each leg [N/m] Leg.k.ax = 2e7; % Stiffness of each leg [N/m]
@ -98,7 +90,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
micro_hexapod = initializeParameters(micro_hexapod); micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg %% Setup equilibrium position of each leg
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB); micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);
%% Save %% Save
save('./mat/stages.mat', 'micro_hexapod', '-append'); save('./mat/stages.mat', 'micro_hexapod', '-append');

24
src/initializeMirror.m
View File

@ -1,15 +1,7 @@
function [] = initializeMirror(opts_param) function [] = initializeMirror(args)
%% Default values for opts arguments
opts = struct(... args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
'shape', 'spherical', ... % spherical or conical args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45
'angle', 45 ...
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -24,7 +16,7 @@ function [] = initializeMirror(opts_param)
mirror.density = 2400; % Density of the mirror [kg/m3] mirror.density = 2400; % Density of the mirror [kg/m3]
mirror.color = [0.4 1.0 1.0]; % Color of the mirror mirror.color = [0.4 1.0 1.0]; % Color of the mirror
mirror.cone_length = mirror.rad*tand(opts.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
%% Shape %% Shape
mirror.shape = [... mirror.shape = [...
@ -34,14 +26,14 @@ function [] = initializeMirror(opts_param)
mirror.rad 0 ... mirror.rad 0 ...
]; ];
if strcmp(opts.shape, 'spherical') if strcmp(args.shape, 'spherical')
mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm] mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]
for z = linspace(0, mirror.h, 101) for z = linspace(0, mirror.h, 101)
mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z]; mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
end end
elseif strcmp(opts.shape, 'conical') elseif strcmp(args.shape, 'conical')
mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.angle) mirror.h]; mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h];
else else
error('Shape should be either conical or spherical'); error('Shape should be either conical or spherical');
end end

26
src/initializeNanoHexapod.m
View File

@ -1,16 +1,8 @@
function [nano_hexapod] = initializeNanoHexapod(opts_param) function [nano_hexapod] = initializeNanoHexapod(args)
%% Default values for opts arguments
opts = struct(... args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
'actuator', 'piezo', ... args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} args.ARB (3,3) double {mustBeNumeric} = eye(3)
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% Stewart Object %% Stewart Object
@ -46,12 +38,12 @@ function [nano_hexapod] = initializeNanoHexapod(opts_param)
Leg = struct(); Leg = struct();
Leg.stroke = 80e-6; % Maximum Stroke of each leg [m] Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
if strcmp(opts.actuator, 'piezo') if strcmp(args.actuator, 'piezo')
Leg.k.ax = 1e7; % Stiffness of each leg [N/m] Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
elseif strcmp(opts.actuator, 'lorentz') elseif strcmp(args.actuator, 'lorentz')
Leg.k.ax = 1e4; % Stiffness of each leg [N/m] Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
else else
error('opts.actuator should be piezo or lorentz'); error('args.actuator should be piezo or lorentz');
end end
Leg.ksi.ax = 10; % Maximum amplification at resonance [] Leg.ksi.ax = 10; % Maximum amplification at resonance []
Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm] Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
@ -101,7 +93,7 @@ function [nano_hexapod] = initializeNanoHexapod(opts_param)
nano_hexapod = initializeParameters(nano_hexapod); nano_hexapod = initializeParameters(nano_hexapod);
%% Setup equilibrium position of each leg %% Setup equilibrium position of each leg
nano_hexapod.L0 = inverseKinematicsHexapod(nano_hexapod, opts.AP, opts.ARB); nano_hexapod.L0 = inverseKinematicsHexapod(nano_hexapod, args.AP, args.ARB);
%% Save %% Save
save('./mat/stages.mat', 'nano_hexapod', '-append'); save('./mat/stages.mat', 'nano_hexapod', '-append');

119
src/initializeReferences.m
View File

@ -1,36 +1,45 @@
function [ref] = initializeReferences(opts_param) function [ref] = initializeReferences(args)
%% Default values for opts arguments
opts = struct( ... % Sampling Frequency [s]
'Ts', 1e-3, ... % Sampling Frequency [s] args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
'Tmax', 100, ... % Maximum simulation time [s] % Maximum simulation time [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
'Dy_amplitude', 0, ... % Amplitude of the displacement [m] % Either "constant" / "triangular" / "sinusoidal"
'Dy_period', 1, ... % Period of the displacement [s] args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" % Amplitude of the displacement [m]
'Ry_amplitude', 0, ... % Amplitude [rad] args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
'Ry_period', 1, ... % Period of the displacement [s] % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating" args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
'Rz_amplitude', 0, ... % Initial angle [rad] % Either "constant" / "triangular" / "sinusoidal"
'Rz_period', 1, ... % Period of the rotating [s] args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
'Dh_type', 'constant', ... % For now, only constant is implemented % Amplitude [rad]
'Dh_pos', zeros(6, 1), ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles) args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
'Rm_type', 'constant', ... % For now, only constant is implemented % Period of the displacement [s]
'Rm_pos', [0; pi], ... % Initial position of the two masses args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
'Dn_type', 'constant', ... % For now, only constant is implemented % Either "constant" / "rotating"
'Dn_pos', zeros(6,1) ... % Initial position [m,m,m,rad,rad,rad] of the top platform args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating'})} = 'constant'
); % Initial angle [rad]
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
%% Populate opts with input parameters % Period of the rotating [s]
if exist('opts_param','var') args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
for opt = fieldnames(opts_param)' % For now, only constant is implemented
opts.(opt{1}) = opts_param.(opt{1}); args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
end % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
% For now, only constant is implemented
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
% Initial position of the two masses
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
% For now, only constant is implemented
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
end end
%% Set Sampling Time %% Set Sampling Time
Ts = opts.Ts; Ts = args.Ts;
Tmax = opts.Tmax; Tmax = args.Tmax;
%% Low Pass Filter to filter out the references %% Low Pass Filter to filter out the references
s = zpk('s'); s = zpk('s');
@ -43,15 +52,15 @@ t = 0:Ts:Tmax; % Time Vector [s]
Dy = zeros(length(t), 1); Dy = zeros(length(t), 1);
Dyd = zeros(length(t), 1); Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1); Dydd = zeros(length(t), 1);
switch opts.Dy_type switch args.Dy_type
case 'constant' case 'constant'
Dy(:) = opts.Dy_amplitude; Dy(:) = args.Dy_amplitude;
Dyd(:) = 0; Dyd(:) = 0;
Dydd(:) = 0; Dydd(:) = 0;
case 'triangular' case 'triangular'
% This is done to unsure that we start with no displacement % This is done to unsure that we start with no displacement
Dy_raw = opts.Dy_amplitude*sawtooth(2*pi*t/opts.Dy_period,1/2); Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
i0 = find(t>=opts.Dy_period/4,1); i0 = find(t>=args.Dy_period/4,1);
Dy(1:end-i0+1) = Dy_raw(i0:end); Dy(1:end-i0+1) = Dy_raw(i0:end);
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
@ -60,9 +69,9 @@ switch opts.Dy_type
Dyd = lsim(H_lpf*s, Dy, t); Dyd = lsim(H_lpf*s, Dy, t);
Dydd = lsim(H_lpf*s^2, Dy, t); Dydd = lsim(H_lpf*s^2, Dy, t);
case 'sinusoidal' case 'sinusoidal'
Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t); Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
Dyd = opts.Dy_amplitude*2*pi/opts.Dy_period*cos(2*pi/opts.Dy_period*t); Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
Dydd = -opts.Dy_amplitude*(2*pi/opts.Dy_period)^2*sin(2*pi/opts.Dy_period*t); Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
otherwise otherwise
warning('Dy_type is not set correctly'); warning('Dy_type is not set correctly');
end end
@ -75,14 +84,14 @@ Ry = zeros(length(t), 1);
Ryd = zeros(length(t), 1); Ryd = zeros(length(t), 1);
Rydd = zeros(length(t), 1); Rydd = zeros(length(t), 1);
switch opts.Ry_type switch args.Ry_type
case 'constant' case 'constant'
Ry(:) = opts.Ry_amplitude; Ry(:) = args.Ry_amplitude;
Ryd(:) = 0; Ryd(:) = 0;
Rydd(:) = 0; Rydd(:) = 0;
case 'triangular' case 'triangular'
Ry_raw = opts.Ry_amplitude*sawtooth(2*pi*t/opts.Ry_period,1/2); Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
i0 = find(t>=opts.Ry_period/4,1); i0 = find(t>=args.Ry_period/4,1);
Ry(1:end-i0+1) = Ry_raw(i0:end); Ry(1:end-i0+1) = Ry_raw(i0:end);
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
@ -91,10 +100,10 @@ switch opts.Ry_type
Ryd = lsim(H_lpf*s, Ry, t); Ryd = lsim(H_lpf*s, Ry, t);
Rydd = lsim(H_lpf*s^2, Ry, t); Rydd = lsim(H_lpf*s^2, Ry, t);
case 'sinusoidal' case 'sinusoidal'
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t); Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
Ryd = opts.Ry_amplitude*2*pi/opts.Ry_period*cos(2*pi/opts.Ry_period*t); Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
Rydd = -opts.Ry_amplitude*(2*pi/opts.Ry_period)^2*sin(2*pi/opts.Ry_period*t); Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
otherwise otherwise
warning('Ry_type is not set correctly'); warning('Ry_type is not set correctly');
end end
@ -107,13 +116,13 @@ Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1); Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1); Rzdd = zeros(length(t), 1);
switch opts.Rz_type switch args.Rz_type
case 'constant' case 'constant'
Rz(:) = opts.Rz_amplitude; Rz(:) = args.Rz_amplitude;
Rzd(:) = 0; Rzd(:) = 0;
Rzdd(:) = 0; Rzdd(:) = 0;
case 'rotating' case 'rotating'
Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t; Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t;
% The signal is filtered out % The signal is filtered out
Rz = lsim(H_lpf, Rz, t); Rz = lsim(H_lpf, Rz, t);
@ -130,17 +139,17 @@ t = [0, Ts];
Dh = zeros(length(t), 6); Dh = zeros(length(t), 6);
Dhl = zeros(length(t), 6); Dhl = zeros(length(t), 6);
switch opts.Dh_type switch args.Dh_type
case 'constant' case 'constant'
Dh = [opts.Dh_pos, opts.Dh_pos]; Dh = [args.Dh_pos, args.Dh_pos];
load('mat/stages.mat', 'micro_hexapod'); load('mat/stages.mat', 'micro_hexapod');
AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)]; AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
tx = opts.Dh_pos(4); tx = args.Dh_pos(4);
ty = opts.Dh_pos(5); ty = args.Dh_pos(5);
tz = opts.Dh_pos(6); tz = args.Dh_pos(6);
ARB = [cos(tz) -sin(tz) 0; ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0; sin(tz) cos(tz) 0;
@ -164,16 +173,16 @@ Dhl = struct('time', t, 'signals', struct('values', Dhl));
%% Axis Compensation - Rm %% Axis Compensation - Rm
t = [0, Ts]; t = [0, Ts];
Rm = [opts.Rm_pos, opts.Rm_pos]; Rm = [args.Rm_pos, args.Rm_pos];
Rm = struct('time', t, 'signals', struct('values', Rm)); Rm = struct('time', t, 'signals', struct('values', Rm));
%% Nano-Hexapod %% Nano-Hexapod
t = [0, Ts]; t = [0, Ts];
Dn = zeros(length(t), 6); Dn = zeros(length(t), 6);
switch opts.Dn_type switch args.Dn_type
case 'constant' case 'constant'
Dn = [opts.Dn_pos, opts.Dn_pos]; Dn = [args.Dn_pos, args.Dn_pos];
otherwise otherwise
warning('Dn_type is not set correctly'); warning('Dn_type is not set correctly');
end end

14
src/initializeRy.m
View File

@ -1,12 +1,6 @@
function [ry] = initializeRy(opts_param) function [ry] = initializeRy(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -33,7 +27,7 @@ function [ry] = initializeRy(opts_param)
ry.m = 800; % TODO [kg] ry.m = 800; % TODO [kg]
%% Tilt Stage - Dynamical Properties %% Tilt Stage - Dynamical Properties
if opts.rigid if args.rigid
ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg] ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg]
ry.k.h = 1e12; % Stiffness in the direction of the guidance [N/m] ry.k.h = 1e12; % Stiffness in the direction of the guidance [N/m]
ry.k.rad = 1e12; % Stiffness in the top direction [N/m] ry.k.rad = 1e12; % Stiffness in the top direction [N/m]

14
src/initializeRz.m
View File

@ -1,12 +1,6 @@
function [rz] = initializeRz(opts_param) function [rz] = initializeRz(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -31,7 +25,7 @@ function [rz] = initializeRz(opts_param)
%% Spindle - Dynamical Properties %% Spindle - Dynamical Properties
if opts.rigid if args.rigid
rz.k.rot = 1e10; % Rotational Stiffness (Rz) [N*m/deg] rz.k.rot = 1e10; % Rotational Stiffness (Rz) [N*m/deg]
rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg] rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg]
rz.k.ax = 1e12; % Axial Stiffness (Z) [N/m] rz.k.ax = 1e12; % Axial Stiffness (Z) [N/m]

21
src/initializeSample.m
View File

@ -1,17 +1,10 @@
function [sample] = initializeSample(opts_param) function [sample] = initializeSample(sample)
%% Default values for opts arguments
sample = struct('radius', 100, ... sample.radius (1,1) double {mustBeNumeric, mustBePositive} = 100
'height', 300, ... sample.height (1,1) double {mustBeNumeric, mustBePositive} = 300
'mass', 50, ... sample.mass (1,1) double {mustBeNumeric, mustBePositive} = 50
'offset', 0, ... sample.offset (1,1) double {mustBeNumeric} = 0
'color', [0.45, 0.45, 0.45] ... sample.color (1,3) double {mustBeNumeric} = [0.45, 0.45, 0.45]
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
sample.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%

14
src/initializeTy.m
View File

@ -1,12 +1,6 @@
function [ty] = initializeTy(opts_param) function [ty] = initializeTy(args)
%% Default values for opts arguments
opts = struct('rigid', false); args.rigid logical {mustBeNumericOrLogical} = false
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end end
%% %%
@ -53,7 +47,7 @@ function [ty] = initializeTy(opts_param)
ty.m = 1000; % TODO [kg] ty.m = 1000; % TODO [kg]
%% Y-Translation - Dynamicals Properties %% Y-Translation - Dynamicals Properties
if opts.rigid if args.rigid
ty.k.ax = 1e12; % Axial Stiffness for each of the 4 guidance (y) [N/m] ty.k.ax = 1e12; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad = 1e12; % Radial Stiffness for each of the 4 guidance (x-z) [N/m] ty.k.rad = 1e12; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
else else

32
uniaxial/index.org
View File

@ -659,8 +659,8 @@ The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0 (Open Loop). All the controllers are set to 0 (Open Loop).
@ -1159,8 +1159,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0. All the controllers are set to 0.
@ -1586,8 +1586,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
And initialize the controllers. And initialize the controllers.
@ -2017,8 +2017,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
And initialize the controllers. And initialize the controllers.
@ -2250,9 +2250,9 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeCedratPiezo(); initializeCedratPiezo();
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
And initialize the controllers. And initialize the controllers.
@ -2386,9 +2386,9 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo')); initializeNanoHexapod('actuator', 'piezo');
initializeCedratPiezo(); initializeCedratPiezo();
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0. All the controllers are set to 0.
@ -2847,8 +2847,8 @@ The nano-hexapod is an hexapod with voice coils and the sample has a mass of 50k
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'lorentz')); initializeNanoHexapod('actuator', 'lorentz');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0 (Open Loop). All the controllers are set to 0 (Open Loop).
@ -3118,8 +3118,8 @@ Let's initialize the system prior to identification.
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(struct('actuator', 'lorentz')); initializeNanoHexapod('actuator', 'lorentz');
initializeSample(struct('mass', 50)); initializeSample('mass', 50);
#+end_src #+end_src
All the controllers are set to 0. All the controllers are set to 0.