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

Correct stupid error

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-04-17 14:32:08 +02:00
parent 6522211e01
commit 00452e0a49
19 changed files with 1835 additions and 2096 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
org/control_virtual_mass.org
View File

@@ -37,62 +37,66 @@
initializeController('type', 'hac-dvf');
#+end_src
We set the stiffness of the payload fixation:
#+begin_src matlab
Kp = 1e8; % [N/m]
#+end_src
* Identification
** Identification of the transfer function from $\tau$ to $d\mathcal{L}$
#+begin_src matlab
K = tf(zeros(6));
Kdvf = tf(zeros(6));
#+end_src
We identify the system for the following payload masses:
#+begin_src matlab
Ms = [1, 10, 50];
#+end_src
#+begin_src matlab :exports none
Gm = {zeros(length(Ms), 1)};
#+end_src
The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
initializeNanoHexapod('k', 1e5, 'c', 2e2);
#+end_src
We set the stiffness of the payload fixation:
#+begin_src matlab
Kp = 1e8; % [N/m]
#+end_src
#+begin_src matlab :exports none
K = tf(zeros(6));
Kdvf = tf(zeros(6));
#+end_src
* Identification
We identify the system for the following payload masses:
#+begin_src matlab
Ms = [1, 10, 50];
#+end_src
Identification of the transfer function from $\tau$ to $d\mathcal{L}$.
#+begin_src matlab :exports none
Gm = {zeros(length(Ms), 1)};
%% 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; % Force Sensors
#+end_src
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
Gm(i) = {G_dvf};
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
Gm(i) = {G};
end
#+end_src
** Identification of the Primary plant without virtual add of mass
Identification of the Primary plant without virtual add of mass
#+begin_src matlab :exports none
G_x = {zeros(length(Ms), 1)};
G_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
%% 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
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
@@ -224,7 +228,7 @@ exportFig('figs/virtual_mass_loop_gain_L.pdf', 'width', 'full', 'height', 'full'
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.J');
@@ -238,7 +242,7 @@ exportFig('figs/virtual_mass_loop_gain_L.pdf', 'width', 'full', 'height', 'full'
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
freqs = logspace(0, 3, 1000);
figure;
@@ -320,7 +324,7 @@ exportFig('figs/virtual_mass_L_primary_plant_X.pdf', 'width', 'full', 'height',
[[file:figs/virtual_mass_L_primary_plant_X.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
freqs = logspace(0, 3, 1000);
figure;
@@ -413,7 +417,7 @@ Let's look at the transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}
yticks([-360:90:360]);
legend('location', 'northeast');
ax1 = subplot(2, 2, 2);
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
@@ -423,7 +427,7 @@ Let's look at the transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 2, 4);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
@@ -437,7 +441,7 @@ Let's look at the transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
@@ -476,9 +480,9 @@ exportFig('figs/virtual_mass_plant_X.pdf', 'width', 'full', 'height', 'full')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 2, 3);
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
LmX = GmX{i}*KmX;
@@ -492,11 +496,11 @@ exportFig('figs/virtual_mass_plant_X.pdf', 'width', 'full', 'height', 'full')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
ylim([-180, 180]);
yticks([-360:90:360]);
legend('location', 'northeast');
legend('location', 'southwest');
ax1 = subplot(2, 2, 2);
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:length(Ms)
LmX = GmX{i}*KmX;
@@ -505,9 +509,9 @@ exportFig('figs/virtual_mass_plant_X.pdf', 'width', 'full', 'height', 'full')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 2, 4);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:length(Ms)
LmX = GmX{i}*KmX;
@@ -518,11 +522,11 @@ exportFig('figs/virtual_mass_plant_X.pdf', 'width', 'full', 'height', 'full')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
ylim([-180, 180]);
yticks([-360:90:360]);
legend('location', 'northeast');
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
@@ -579,7 +583,7 @@ exportFig('figs/virtual_mass_loop_gain_X.pdf', 'width', 'full', 'height', 'full'
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
freqs = logspace(0, 3, 1000);
figure;
@@ -661,7 +665,7 @@ exportFig('figs/virtual_mass_X_primary_plant_X.pdf', 'width', 'full', 'height',
[[file:figs/virtual_mass_X_primary_plant_X.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
freqs = logspace(0, 3, 1000);
figure;