diff --git a/org/control_hac_lac.org b/org/control_hac_lac.org new file mode 100644 index 0000000..e8803dd --- /dev/null +++ b/org/control_hac_lac.org @@ -0,0 +1,529 @@ +#+TITLE: HAC-LAC applied on the Simscape Model +:DRAWER: +#+STARTUP: overview + +#+LANGUAGE: en +#+EMAIL: dehaeze.thomas@gmail.com +#+AUTHOR: Dehaeze Thomas + +#+HTML_LINK_HOME: ./index.html +#+HTML_LINK_UP: ./index.html + +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: + +#+HTML_MATHJAX: align: center tagside: right font: TeX + +#+PROPERTY: header-args:matlab :session *MATLAB* +#+PROPERTY: header-args:matlab+ :comments org +#+PROPERTY: header-args:matlab+ :results none +#+PROPERTY: header-args:matlab+ :exports both +#+PROPERTY: header-args:matlab+ :eval no-export +#+PROPERTY: header-args:matlab+ :output-dir figs +#+PROPERTY: header-args:matlab+ :tangle no +#+PROPERTY: header-args:matlab+ :mkdirp yes + +#+PROPERTY: header-args:shell :eval no-export + +#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}") +#+PROPERTY: header-args:latex+ :imagemagick t :fit yes +#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 +#+PROPERTY: header-args:latex+ :imoutoptions -quality 100 +#+PROPERTY: header-args:latex+ :results file raw replace +#+PROPERTY: header-args:latex+ :buffer no +#+PROPERTY: header-args:latex+ :eval no-export +#+PROPERTY: header-args:latex+ :exports results +#+PROPERTY: header-args:latex+ :mkdirp yes +#+PROPERTY: header-args:latex+ :output-dir figs +#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png") +:END: + +* Introduction :ignore: +The position $\bm{\mathcal{X}}$ of the Sample with respect to the granite is measured. + +It is then compare to the wanted position of the Sample $\bm{r}_\mathcal{X}$ in order to obtain the position error $\bm{\epsilon}_\mathcal{X}$ of the Sample with respect to a frame attached to the Stewart top platform. + +#+begin_src latex :file hac_lac_control_schematic.pdf + \begin{tikzpicture} + \node[block={3.0cm}{3.0cm}] (G) {Plant}; + + % Input and outputs coordinates + \coordinate[] (outputX) at ($(G.south east)!0.25!(G.north east)$); + \coordinate[] (outputL) at ($(G.south east)!0.75!(G.north east)$); + + \draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$}; + \draw[->] (outputL) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{L}}$}; + + % Blocs + \node[addb, left= of G] (addF) {}; + \node[block, left=1.2 of addF] (Kx) {$\bm{K}_\mathcal{X}$}; + \node[block={2cm}{2cm}, align=center, left=1.2 of Kx] (subx) {Computes\\Position\\Error}; + + \node[block, above= of addF] (Kl) {$\bm{K}_\mathcal{L}$}; + \node[addb={+}{}{}{-}{}, above= of Kl] (subl) {}; + + \node[block, align=center, left=0.8 of subl] (invK) {Inverse\\Kinematics}; + + % Connections and labels + \draw[<-] (subx.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-0.8, 0); + \draw[->] ($(subx.east) + (0.2, 0)$)node[branch]{} |- (invK.west); + \draw[->] (invK.east) -- (subl.west) node[above left]{$\bm{r}_\mathcal{L}$}; + \draw[->] (subl.south) -- (Kl.north) node[above right]{$\bm{\epsilon}_\mathcal{L}$}; + \draw[->] (Kl.south) -- (addF.north); + + \draw[->] (subx.east) -- (Kx.west) node[above left]{$\bm{\epsilon}_\mathcal{X}$}; + \draw[->] (Kx.east) node[above right]{$\bm{\tau}_\mathcal{X}$} -- (addF.west); + \draw[->] (addF.east) -- (G.west) node[above left]{$\bm{\tau}$}; + + \draw[->] ($(outputL.east) + (0.4, 0)$)node[branch](L){} |- (subl.east); + \draw[->] ($(outputX.east) + (1.2, 0)$)node[branch]{} -- ++(0, -1.6) -| (subx.south); + + \begin{scope}[on background layer] + \node[fit={(G.south-|Kl.west) (L|-subl.north)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Ktot) {}; + \end{scope} + \end{tikzpicture} +#+end_src + +#+name: fig:hac_lac_control_schematic +#+caption: HAC-LAC Control Architecture used for the Control of the NASS +#+RESULTS: +[[file:figs/hac_lac_control_schematic.png]] + +* Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no + simulinkproject('../'); +#+end_src + +#+begin_src matlab + open('nass_model.slx') +#+end_src + +* Initialization +We initialize all the stages with the default parameters. +#+begin_src matlab + initializeGround(); + initializeGranite(); + initializeTy(); + initializeRy(); + initializeRz(); + initializeMicroHexapod(); + initializeAxisc(); + initializeMirror(); +#+end_src + +The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg. +#+begin_src matlab + initializeNanoHexapod('actuator', 'piezo'); + initializeSample('mass', 1); +#+end_src + +We set the references that corresponds to a tomography experiment. +#+begin_src matlab + initializeReferences('Rz_type', 'rotating', 'Rz_period', 1); +#+end_src + +#+begin_src matlab + initializeDisturbances(); +#+end_src + +Open Loop. +#+begin_src matlab + initializeController('type', 'open-loop'); +#+end_src + +And we put some gravity. +#+begin_src matlab + initializeSimscapeConfiguration('gravity', true); +#+end_src + +We log the signals. +#+begin_src matlab + initializeLoggingConfiguration('log', 'all'); +#+end_src + +* Low Authority Control - Direct Velocity Feedback $\bm{K}_\mathcal{L}$ +** Introduction :ignore: +The first loop closed corresponds to a direct velocity feedback loop. + +The design of the associated decentralized controller is explained in [[file:control_active_damping.org][this]] file. + +** Identification +#+begin_src matlab + %% Name of the Simulink File + mdl = 'nass_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs + io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Relative Motion Outputs + + %% Run the linearization + G_dvf = linearize(mdl, io, 0); + G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}; +#+end_src + +** Plant +#+begin_src matlab :exports none + freqs = logspace(0, 3, 1000); + + figure; + + ax1 = subplot(2, 2, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + title('Diagonal elements of the Plant'); + + ax2 = subplot(2, 2, 3); + hold on; + for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i)); + 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]); + legend('location', 'northwest'); + + ax3 = subplot(2, 2, 2); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz')))); + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + title('Off-Diagonal elements of the Plant'); + + ax4 = subplot(2, 2, 4); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(1, 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,ax3,ax4],'x'); +#+end_src + +** Root Locus +#+begin_src matlab :exports none + gains = logspace(0, 5, 500); + + figure; + hold on; + plot(real(pole(G_dvf)), imag(pole(G_dvf)), 'x'); + set(gca,'ColorOrderIndex',1); + plot(real(tzero(G_dvf)), imag(tzero(G_dvf)), 'o'); + for i = 1:length(gains) + set(gca,'ColorOrderIndex',1); + cl_poles = pole(feedback(G_dvf, (gains(i)*s)*eye(6))); + plot(real(cl_poles), imag(cl_poles), '.'); + end + ylim([0, 2*pi*500]); + xlim([-2*pi*500,0]); + xlabel('Real Part') + ylabel('Imaginary Part') + axis square +#+end_src + +** Controller and Loop Gain +#+begin_src matlab + K_dvf = s*15000/(1 + s/2/pi/10000); +#+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(K_dvf*G_dvf(i,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(K_dvf*G_dvf(i,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 + K_dvf = -K_dvf*eye(6); +#+end_src + +* High Authority Control - $\bm{K}_\mathcal{X}$ +** Identification of the damped plant +#+begin_src matlab + Kx = tf(zeros(6)); +#+end_src + +#+begin_src matlab + initializeController('type', 'hac-dvf'); +#+end_src + +#+begin_src matlab + %% Name of the Simulink File + mdl = 'nass_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs + io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror + + %% Run the linearization + G = linearize(mdl, io, 0); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; +#+end_src + +The minus sine is put here because there is already a minus sign included due to the computation of the position error. +#+begin_src matlab + load('mat/stages.mat', 'nano_hexapod'); + + Gx = -G*inv(nano_hexapod.J'); + Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +#+end_src + +#+begin_src matlab :exports none + freqs = logspace(0, 3, 1000); + + labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; + + figure; + + ax1 = subplot(2, 2, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + title('Diagonal elements of the Plant'); + + ax2 = subplot(2, 2, 3); + hold on; + for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i}); + 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]); + legend(); + + ax3 = subplot(2, 2, 2); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz')))); + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + title('Off-Diagonal elements of the Plant'); + + ax4 = subplot(2, 2, 4); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 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,ax3,ax4],'x'); +#+end_src + +** Controller Design +The controller consists of: +- A pure integrator +- A Second integrator up to half the wanted bandwidth +- A Lead around the cross-over frequency +- A low pass filter with a cut-off equal to two times the wanted bandwidth + +#+begin_src matlab + wc = 2*pi*15; % Bandwidth Bandwidth [rad/s] + + h = 1.5; % Lead parameter + + Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/2)); + + % Normalization of the gain of have a loop gain of 1 at frequency wc + Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc)))); +#+end_src + +#+begin_src matlab :exports none + freqs = logspace(0, 3, 1000); + + labels = {'$L_x$', '$L_y$', '$L_z$', '$L_{R_x}$', '$L_{R_y}$', '$L_{R_z}$'}; + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + title('Diagonal elements of the Plant'); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))), 'DisplayName', labels{i}); + 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]); + legend(); + + linkaxes([ax1,ax2],'x'); +#+end_src + +#+begin_src matlab + isstable(feedback(Gx*Kx, eye(6), -1)) +#+end_src + +#+begin_src matlab + Kx = inv(nano_hexapod.J')*Kx; +#+end_src + +#+begin_src matlab + isstable(feedback(G*Kx, eye(6), 1)) +#+end_src + +* Simulation +#+begin_src matlab + load('mat/conf_simulink.mat'); + set_param(conf_simulink, 'StopTime', '2'); +#+end_src + +And we simulate the system. +#+begin_src matlab + sim('nass_model'); +#+end_src + +#+begin_src matlab + hac_dvf = simout; + save('./mat/tomo_exp_hac_lac.mat', 'hac_dvf'); +#+end_src + +* Results +Let's load the simulation when no control is applied. +#+begin_src matlab + load('./mat/experiment_tomography.mat', 'tomo_align_dist'); + load('./mat/tomo_exp_hac_lac.mat', 'hac_dvf'); +#+end_src + +#+begin_src matlab :exports none + figure; + ax1 = subplot(2, 3, 1); + hold on; + plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1)) + plot(hac_dvf.Em.En.Time, hac_dvf.Em.En.Data(:, 1)) + hold off; + xlabel('Time [s]'); + ylabel('Dx [m]'); + + ax2 = subplot(2, 3, 2); + hold on; + plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2)) + plot(hac_dvf.Em.En.Time, hac_dvf.Em.En.Data(:, 2)) + hold off; + xlabel('Time [s]'); + ylabel('Dy [m]'); + + ax3 = subplot(2, 3, 3); + hold on; + plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3)) + plot(hac_dvf.Em.En.Time, hac_dvf.Em.En.Data(:, 3)) + hold off; + xlabel('Time [s]'); + ylabel('Dz [m]'); + + ax4 = subplot(2, 3, 4); + hold on; + plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4)) + plot(hac_dvf.Em.En.Time, hac_dvf.Em.En.Data(:, 4)) + hold off; + xlabel('Time [s]'); + ylabel('Rx [rad]'); + + ax5 = subplot(2, 3, 5); + hold on; + plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5)) + plot(hac_dvf.Em.En.Time, hac_dvf.Em.En.Data(:, 5)) + hold off; + xlabel('Time [s]'); + ylabel('Ry [rad]'); + + ax6 = subplot(2, 3, 6); + hold on; + plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station') + plot(hac_dvf.Em.En.Time, hac_dvf.Em.En.Data(:, 6), 'DisplayName', 'HAC-DVF') + hold off; + xlabel('Time [s]'); + ylabel('Rz [rad]'); + legend(); + + linkaxes([ax1,ax2,ax3,ax4],'x'); + xlim([0.5, inf]); +#+end_src diff --git a/org/hac_lac.org b/org/hac_lac.org deleted file mode 100644 index 0b6c299..0000000 --- a/org/hac_lac.org +++ /dev/null @@ -1,1099 +0,0 @@ -#+TITLE: HAC-LAC applied on the Simscape Model -:DRAWER: -#+STARTUP: overview - -#+LANGUAGE: en -#+EMAIL: dehaeze.thomas@gmail.com -#+AUTHOR: Dehaeze Thomas - -#+HTML_LINK_HOME: ./index.html -#+HTML_LINK_UP: ./index.html - -#+HTML_HEAD: -#+HTML_HEAD: -#+HTML_HEAD: -#+HTML_HEAD: -#+HTML_HEAD: -#+HTML_HEAD: -#+HTML_HEAD: - -#+HTML_MATHJAX: align: center tagside: right font: TeX - -#+PROPERTY: header-args:matlab :session *MATLAB* -#+PROPERTY: header-args:matlab+ :comments org -#+PROPERTY: header-args:matlab+ :results none -#+PROPERTY: header-args:matlab+ :exports both -#+PROPERTY: header-args:matlab+ :eval no-export -#+PROPERTY: header-args:matlab+ :output-dir figs -#+PROPERTY: header-args:matlab+ :tangle no -#+PROPERTY: header-args:matlab+ :mkdirp yes - -#+PROPERTY: header-args:shell :eval no-export - -#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}") -#+PROPERTY: header-args:latex+ :imagemagick t :fit yes -#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 -#+PROPERTY: header-args:latex+ :imoutoptions -quality 100 -#+PROPERTY: header-args:latex+ :results file raw replace -#+PROPERTY: header-args:latex+ :buffer no -#+PROPERTY: header-args:latex+ :eval no-export -#+PROPERTY: header-args:latex+ :exports results -#+PROPERTY: header-args:latex+ :mkdirp yes -#+PROPERTY: header-args:latex+ :output-dir figs -#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png") -:END: - -* Introduction :ignore: -The position $\bm{\mathcal{X}}$ of the Sample with respect to the granite is measured. - -It is then compare to the wanted position of the Sample $\bm{r}_\mathcal{X}$ in order to obtain the position error $\bm{\epsilon}_\mathcal{X}$ of the Sample with respect to a frame attached to the Stewart top platform. - -#+begin_src latex :file hac_lac_control_schematic.pdf - \begin{tikzpicture} - \node[block={3.0cm}{3.0cm}] (G) {$G$}; - - % Input and outputs coordinates - \coordinate[] (outputX) at ($(G.south east)!0.25!(G.north east)$); - \coordinate[] (outputL) at ($(G.south east)!0.75!(G.north east)$); - - \draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$}; - \draw[->] (outputL) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{L}}$}; - - % Blocs - \node[addb, left= of G] (addF) {}; - \node[block, left=1.2 of addF] (Kx) {$\bm{K}_\mathcal{X}$}; - \node[block={2cm}{2cm}, align=center, left=1.2 of Kx] (subx) {Computes\\Position\\Error}; - - \node[block, above= of addF] (Kl) {$\bm{K}_\mathcal{L}$}; - \node[addb={+}{}{}{-}{}, above= of Kl] (subl) {}; - - \node[block, align=center, left=0.8 of subl] (invK) {Inverse\\Kinematics}; - - % Connections and labels - \draw[<-] (subx.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-0.8, 0); - \draw[->] ($(subx.east) + (0.2, 0)$)node[branch]{} |- (invK.west); - \draw[->] (invK.east) -- (subl.west) node[above left]{$\bm{r}_\mathcal{L}$}; - \draw[->] (subl.south) -- (Kl.north) node[above right]{$\bm{\epsilon}_\mathcal{L}$}; - \draw[->] (Kl.south) -- (addF.north); - - \draw[->] (subx.east) -- (Kx.west) node[above left]{$\bm{\epsilon}_\mathcal{X}$}; - \draw[->] (Kx.east) node[above right]{$\bm{\tau}_\mathcal{X}$} -- (addF.west); - \draw[->] (addF.east) -- (G.west) node[above left]{$\bm{\tau}$}; - - \draw[->] ($(outputL.east) + (0.4, 0)$)node[branch](L){} |- (subl.east); - \draw[->] ($(outputX.east) + (1.2, 0)$)node[branch]{} -- ++(0, -1.6) -| (subx.south); - - \begin{scope}[on background layer] - \node[fit={(G.south-|Kl.west) (L|-subl.north)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Ktot) {}; - \end{scope} - \end{tikzpicture} -#+end_src - -#+name: fig:hac_lac_control_schematic -#+caption: HAC-LAC Control Architecture used for the Control of the NASS -#+RESULTS: -[[file:figs/hac_lac_control_schematic.png]] - -* Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) -<> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes -<> -#+end_src - -#+begin_src matlab :tangle no - simulinkproject('../'); -#+end_src - -#+begin_src matlab - open('nass_model.slx') -#+end_src - -* Initialization -We initialize all the stages with the default parameters. -#+begin_src matlab - initializeGround(); - initializeGranite(); - initializeTy(); - initializeRy(); - initializeRz(); - initializeMicroHexapod(); - initializeAxisc(); - initializeMirror(); -#+end_src - -The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg. -#+begin_src matlab - initializeNanoHexapod('actuator', 'piezo'); - initializeSample('mass', 1); -#+end_src - -We set the references that corresponds to a tomography experiment. -#+begin_src matlab - initializeReferences('Rz_type', 'rotating', 'Rz_period', 1); -#+end_src - -#+begin_src matlab - initializeDisturbances(); -#+end_src - -Open Loop. -#+begin_src matlab - initializeController('type', 'open-loop'); -#+end_src - -And we put some gravity. -#+begin_src matlab - initializeSimscapeConfiguration('gravity', true); -#+end_src - -We log the signals. -#+begin_src matlab - initializeLoggingConfiguration('log', 'all'); -#+end_src - -* Low Authority Control - Direct Velocity Feedback $\bm{K}_\mathcal{L}$ -** Introduction :ignore: -The first loop closed corresponds to a direct velocity feedback loop. - -The design of the associated decentralized controller is explained in [[file:active_damping.org][this]] file. - -** Identification -#+begin_src matlab - %% Name of the Simulink File - mdl = 'nass_model'; - - %% Input/Output definition - clear io; io_i = 1; - io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs - io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Relative Motion Outputs - - %% Run the linearization - G_dvf = linearize(mdl, io, 0); - G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; - G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}; -#+end_src - -** Plant -#+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_dvf(i,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_dvf(i,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 - -** Root Locus -#+begin_src matlab :exports none - gains = logspace(0, 5, 500); - - figure; - hold on; - plot(real(pole(G_dvf)), imag(pole(G_dvf)), 'x'); - set(gca,'ColorOrderIndex',1); - plot(real(tzero(G_dvf)), imag(tzero(G_dvf)), 'o'); - for i = 1:length(gains) - set(gca,'ColorOrderIndex',1); - cl_poles = pole(feedback(G_dvf, (gains(i)*s)*eye(6))); - plot(real(cl_poles), imag(cl_poles), '.'); - end - % ylim([0, 1.1*max(imag(pole(G_dvf)))]); - % xlim([-1.1*max(imag(pole(G_dvf))),0]); - xlabel('Real Part') - ylabel('Imaginary Part') - axis square -#+end_src - -** Controller and Loop Gain -#+begin_src matlab - K_dvf = s*15000/(1 + s/2/pi/10000); -#+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(K_dvf*G_dvf(i,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(K_dvf*G_dvf(i,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 - K_dvf = -K_dvf*eye(6); -#+end_src - -* High Authority Control - $\bm{K}_\mathcal{X}$ -** Identification of the damped plant -#+begin_src matlab - Kx = tf(zeros(6)); -#+end_src - -#+begin_src matlab - initializeController('type', 'hac-dvf'); -#+end_src - -#+begin_src matlab - %% Name of the Simulink File - mdl = 'nass_model'; - - %% Input/Output definition - clear io; io_i = 1; - io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs - io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror - - %% Run the linearization - G = linearize(mdl, io, 0); - G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; - G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; -#+end_src - -The minus sine is put here because there is already a minus sign included due to the computation of the position error. -#+begin_src matlab - load('mat/stages.mat', 'nano_hexapod'); - - Gx = -G*inv(nano_hexapod.J'); - Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; -#+end_src - -#+begin_src matlab :exports none - freqs = logspace(0, 3, 1000); - - labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; - - figure; - - ax1 = subplot(2, 2, 1); - hold on; - for i = 1:6 - plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz')))); - end - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); - title('Diagonal elements of the Plant'); - - ax2 = subplot(2, 2, 3); - hold on; - for i = 1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i}); - 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]); - legend(); - - ax3 = subplot(2, 2, 2); - hold on; - for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end - end - set(gca,'ColorOrderIndex',1); - plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz')))); - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); - title('Off-Diagonal elements of the Plant'); - - ax4 = subplot(2, 2, 4); - hold on; - for i = 1:5 - for j = i+1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end - end - set(gca,'ColorOrderIndex',1); - plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 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,ax3,ax4],'x'); -#+end_src - -** Controller Design -The controller consists of: -- A pure integrator -- A Second integrator up to half the wanted bandwidth -- A Lead around the cross-over frequency -- A low pass filter with a cut-off equal to two times the wanted bandwidth - -#+begin_src matlab - wc = 2*pi*15; % Bandwidth Bandwidth [rad/s] - - h = 1.5; % Lead parameter - - Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/2)); - - % Normalization of the gain of have a loop gain of 1 at frequency wc - Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc)))); -#+end_src - -#+begin_src matlab :exports none - freqs = logspace(0, 3, 1000); - - labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; - - figure; - - ax1 = subplot(2, 2, 1); - hold on; - for i = 1:6 - plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz')))); - end - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); - title('Diagonal elements of the Plant'); - - ax2 = subplot(2, 2, 3); - hold on; - for i = 1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))), 'DisplayName', labels{i}); - 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]); - legend(); - - ax3 = subplot(2, 2, 2); - hold on; - for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gx(i, j)*Kx(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end - end - set(gca,'ColorOrderIndex',1); - plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz')))); - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); - title('Off-Diagonal elements of the Plant'); - - ax4 = subplot(2, 2, 4); - hold on; - for i = 1:5 - for j = i+1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end - end - set(gca,'ColorOrderIndex',1); - plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 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,ax3,ax4],'x'); -#+end_src - -#+begin_src matlab - isstable(feedback(Gx*Kx, eye(6), -1)) -#+end_src - -#+begin_src matlab - Kx = inv(nano_hexapod.J')*Kx; -#+end_src - -#+begin_src matlab - isstable(feedback(G*Kx, eye(6), 1)) -#+end_src - -* Simulation -#+begin_src matlab - load('mat/conf_simulink.mat'); - set_param(conf_simulink, 'StopTime', '1.5'); -#+end_src - -And we simulate the system. -#+begin_src matlab - sim('nass_model'); -#+end_src - -#+begin_src matlab - save('./mat/tomo_exp_hac_lac.mat', 'simout'); -#+end_src - -* Results -#+begin_src matlab - load('./mat/tomo_exp_hac_lac.mat', 'simout'); -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - plot(simout.Em.En.Data(:,1), simout.Em.En.Data(:,2), 'DisplayName', '$\epsilon_{x,y}$ - OL') - xlabel('X Motion [m]'); ylabel('Y Motion [m]'); - legend(); -#+end_src - - -#+begin_src matlab :exports none - figure; - hold on; - plot3(simout.Em.En.Data(:,1), simout.Em.En.Data(:,2), simout.Em.En.Data(:,3)) - xlabel('X Motion [m]'); ylabel('Y Motion [m]'); -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - plot3(simout.Em.En.Data(:,4), simout.Em.En.Data(:,5), simout.Em.En.Data(:,3)) - xlabel('X Motion [m]'); ylabel('Y Motion [m]'); -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - plot(simout.Em.En.Time, simout.Em.En.Data(:,1), 'DisplayName', '$\epsilon_{x}$') - plot(simout.Em.En.Time, simout.Em.En.Data(:,2), 'DisplayName', '$\epsilon_{y}$') - plot(simout.Em.En.Time, simout.Em.En.Data(:,3), 'DisplayName', '$\epsilon_{z}$') - hold off; - legend(); - xlabel('Time [s]'); ylabel('Position Error [m]'); -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - plot(simout.Em.En.Time, simout.Em.En.Data(:,4), 'DisplayName', '$\epsilon_{R_x}$') - plot(simout.Em.En.Time, simout.Em.En.Data(:,5), 'DisplayName', '$\epsilon_{R_y}$') - plot(simout.Em.En.Time, simout.Em.En.Data(:,6), 'DisplayName', '$\epsilon_{R_z}$') - hold off; - legend(); - xlabel('Time [s]'); ylabel('Orientation Error [rad]'); -#+end_src - -* Undamped System :noexport: -<> - -** Introduction :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) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -#+begin_src matlab :tangle no - simulinkproject('../'); -#+end_src - -#+begin_src matlab - open('nass_model.slx') -#+end_src - -** Identification of the plant -*** Initialize the Simulation -We initialize all the stages with the default parameters. -#+begin_src matlab - initializeGround(); - initializeGranite(); - initializeTy(); - initializeRy(); - initializeRz(); - initializeMicroHexapod(); - initializeAxisc(); - initializeMirror(); -#+end_src - -The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg. -#+begin_src matlab - initializeNanoHexapod('actuator', 'piezo'); - initializeSample('mass', 50); -#+end_src - -No disturbances. -#+begin_src matlab - initializeDisturbances(); -#+end_src - -We set the references to zero. -#+begin_src matlab - initializeReferences(); -#+end_src - -And all the controllers are set to 0. -#+begin_src matlab - initializeController('type', 'open-loop'); -#+end_src - -*** Identification -First, we identify the dynamics of the system using the =linearize= function. -#+begin_src matlab - %% Options for Linearized - options = linearizeOptions; - options.SampleTime = 0; - - %% Name of the Simulink File - mdl = 'nass_model'; - - %% Input/Output definition - clear io; io_i = 1; - io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs - io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs - - %% 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 = minreal(G*inv(nano_hexapod.J')); - G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}; -#+end_src - -#+begin_src matlab - G_legs = minreal(inv(nano_hexapod.J)*G); - G_legs.OutputName = {'e1', 'e2', 'e3', 'e4', 'e5', 'e6'}; -#+end_src - -*** Display TF -#+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_cart(i, 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_cart(i, i), freqs, 'Hz'))), 'DisplayName', [G_cart.InputName{i}, ' to ', G_cart.OutputName{i}]); - 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]); - legend('location', 'southwest'); - - linkaxes([ax1,ax2],'x'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/plant_G_cart.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:plant_G_cart -#+CAPTION: Transfer Function from forces applied by the nano-hexapod to position error ([[./figs/plant_G_cart.png][png]], [[./figs/plant_G_cart.pdf][pdf]]) -[[file:figs/plant_G_cart.png]] - -#+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_legs(['e', 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_legs(['e', 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 - -*** Obtained Plants 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_iff(['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_iff(['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 - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_active_damping_iff_plant -#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]]) -[[file:figs/nass_active_damping_iff_plant.png]] - -#+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_dvf(['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_dvf(['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 - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_active_damping_dvf_plant -#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]]) -[[file:figs/nass_active_damping_ine_plant.png]] - -#+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_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz')))); - end - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]); - - ax2 = subplot(2, 1, 2); - hold on; - for i = 1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', 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 - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_active_damping_inertial_plant -#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]]) -[[file:figs/nass_active_damping_inertial_plant.png]] - -** Tomography Experiment -*** Simulation -We initialize elements for the tomography experiment. -#+begin_src matlab - prepareTomographyExperiment(); -#+end_src - -We change the simulation stop time. -#+begin_src matlab - load('mat/conf_simulink.mat'); - set_param(conf_simulink, 'StopTime', '3'); -#+end_src - -And we simulate the system. -#+begin_src matlab - sim('nass_model'); -#+end_src - -Finally, we save the simulation results for further analysis -#+begin_src matlab - save('./mat/active_damping_tomo_exp.mat', 'En', 'Eg', '-append'); -#+end_src - -*** Results -We load the results of tomography experiments. -#+begin_src matlab - load('./mat/active_damping_tomo_exp.mat', 'En'); - t = linspace(0, 3, length(En(:,1))); -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$') - plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$') - plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$') - hold off; - legend(); - xlabel('Time [s]'); ylabel('Position Error [m]'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_act_damp_undamped_sim_tomo_trans -#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_undamped_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_trans.pdf][pdf]]) -[[file:figs/nass_act_damp_undamped_sim_tomo_trans.png]] - -#+begin_src matlab :exports none - figure; - hold on; - plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$') - plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$') - plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$') - hold off; - xlim([0.5,inf]); - legend(); - xlabel('Time [s]'); ylabel('Position Error [rad]'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_act_damp_undamped_sim_tomo_rot -#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_undamped_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_rot.pdf][pdf]]) -[[file:figs/nass_act_damp_undamped_sim_tomo_rot.png]] - -** Verification of the transfer function from nano hexapod to metrology -*** Initialize the Simulation -We initialize all the stages with the default parameters. -#+begin_src matlab - initializeGround(); - initializeGranite(); - initializeTy(); - initializeRy(); - initializeRz(); - initializeMicroHexapod(); - initializeAxisc(); - initializeMirror(); -#+end_src - -The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg. -#+begin_src matlab - initializeNanoHexapod('actuator', 'piezo'); - initializeSample('mass', 50); -#+end_src - -No disturbances. -#+begin_src matlab - initializeDisturbances('enable', false); -#+end_src - -We set the references to zero. -#+begin_src matlab - initializeReferences(); -#+end_src - -And all the controllers are set to 0. -#+begin_src matlab - initializeController('type', 'open-loop'); -#+end_src - -*** Identification -First, we identify the dynamics of the system using the =linearize= function. -#+begin_src matlab - %% Options for Linearized - options = linearizeOptions; - options.SampleTime = 0; - - %% Name of the Simulink File - mdl = 'nass_model'; - - %% Input/Output definition - clear io; io_i = 1; - io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs - io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs - - %% 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 = minreal(G*inv(nano_hexapod.J')); - G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}; -#+end_src - -#+begin_src matlab - G_legs = minreal(inv(nano_hexapod.J)*G); - G_legs.OutputName = {'e1', 'e2', 'e3', 'e4', 'e5', 'e6'}; -#+end_src - -*** Display TF -#+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_cart(i, 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_cart(i, i), freqs, 'Hz'))), 'DisplayName', [G_cart.InputName{i}, ' to ', G_cart.OutputName{i}]); - 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]); - legend(); - - linkaxes([ax1,ax2],'x'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/plant_G_cart.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:plant_G_cart -#+CAPTION: Transfer Function from forces applied by the nano-hexapod to position error ([[./figs/plant_G_cart.png][png]], [[./figs/plant_G_cart.pdf][pdf]]) -[[file:figs/plant_G_cart.png]] - -#+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_legs(['e', 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_legs(['e', 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 - -*** Obtained Plants 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_iff(['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_iff(['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 - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_active_damping_iff_plant -#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]]) -[[file:figs/nass_active_damping_iff_plant.png]] - -#+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_dvf(['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_dvf(['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 - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_active_damping_dvf_plant -#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]]) -[[file:figs/nass_active_damping_ine_plant.png]] - -#+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_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz')))); - end - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]); - - ax2 = subplot(2, 1, 2); - hold on; - for i = 1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', 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 - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") -<> -#+end_src - -#+NAME: fig:nass_active_damping_inertial_plant -#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]]) -[[file:figs/nass_active_damping_inertial_plant.png]]