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

Align the centers of rotation of Ry, Hexa, NASS with the sample

Browse Source
This commit is contained in:
Thomas Dehaeze 2018-10-30 14:37:19 +01:00
parent ef6f4613b1
commit 389fd66ba4
27 changed files with 90 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
control/control_cl_sim.m
View File

@ -9,3 +9,7 @@ runSimulation('vc', 'light', 'cl', 'none');
runSimulation('pz', 'light', 'cl', 'none');
% runSimulation('vc', 'heavy', 'cl', 'none');
% runSimulation('pz', 'heavy', 'cl', 'none');
%%
opts_inputs = struct('Dw', true);
initializeInputs(opts_inputs);

8
control/control_cl_tf.m
View File

@ -9,12 +9,12 @@ initializeSample(struct('mass', 1));
initializeNanoHexapod(struct('actuator', 'lorentz'));
K = K_light_vc; %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
Gd_cl_light_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
K = K_light_pz; %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
Gd_cl_light_pz = identifyPlant();
%% Closed Loop - Heavy Sample
@ -22,12 +22,12 @@ initializeSample(struct('mass', 50));
initializeNanoHexapod(struct('actuator', 'lorentz'));
K = K_heavy_vc; %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
G_cl_heavy_vc = identifyPlant();
initializeNanoHexapod(struct('actuator', 'piezo'));
K = K_heavy_pz;
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
G_cl_heavy_pz = identifyPlant();
%% Save the identified transfer functions

52
control/control_generate.m
View File

@ -5,13 +5,53 @@ clear; close all; clc;
load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
%%
fs = 10;
s = tf('s');
K_light_vc = generateDiagPidControl(G_light_vc.G_cart, fs);
K_light_pz = generateDiagPidControl(G_light_pz.G_cart, fs);
%%
% bodeFig({minreal(G_light_vc.G_cart(1, 1))}, struct('phase', true));
K_heavy_vc = generateDiagPidControl(G_heavy_vc.G_cart, fs);
K_heavy_pz = generateDiagPidControl(G_heavy_pz.G_cart, fs);
%%
% sisotool(minreal(G_light_vc.G_cart(6, 6)))
K_light_vc = tf(zeros(6));
K_light_vc(1, 1) = 3.4802e06*(s+0.6983)*(s+131.6)/((s+5.306e-06)*(s+577.4));
K_light_vc(2, 2) = 5.3292e06*(s+0.6722)*(s+58.23)/((s+3.628e-06)*(s+734.6));
K_light_vc(3, 3) = 2.1727e06*(s+66.94)*(s+3.522)/((s+530.6)*(s+0.0006859));
K_light_vc(4, 4) = 6.4807e06*(s+2.095)*(s+111.1)/((s+0.0001246)*(s+577.4));
K_light_vc(5, 5) = 2.5184e06*(s+5.582)*(s+48.76)/((s+0.01558)*(s+453.8));
K_light_vc(6, 6) = 1.022e06*(s+6.152)*(s+40.15)/((s+0.001945)*(s+589.7));
%%
i = 6;
bodeFig({-G_light_vc.G_plant(i, i)*K_light_vc(i, i)}, struct('phase', true))
%%
% sisotool(minreal(G_light_pz.G_plant(1, 1)))
K_light_pz = tf(zeros(6));
% K_light_pz(1, 1) = 3.4802e06*(s+0.6983)*(s+131.6)/((s+5.306e-06)*(s+577.4));
% K_light_pz(2, 2) = 5.3292e06*(s+0.6722)*(s+58.23)/((s+3.628e-06)*(s+734.6));
% K_light_pz(3, 3) = 2.1727e06*(s+66.94)*(s+3.522)/((s+530.6)*(s+0.0006859));
% K_light_pz(4, 4) = 6.4807e06*(s+2.095)*(s+111.1)/((s+0.0001246)*(s+577.4));
% K_light_pz(5, 5) = 2.5184e06*(s+5.582)*(s+48.76)/((s+0.01558)*(s+453.8));
% K_light_pz(6, 6) = 1.022e06*(s+6.152)*(s+40.15)/((s+0.001945)*(s+589.7));
%%
K_heavy_vc = tf(zeros(6));
K_heavy_pz = tf(zeros(6));
%%
save('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');
%% Automatic generation of controllers
% fs = 100;
%
% K_light_vc = generateDiagPidControl(G_light_vc.G_cart, fs);
% K_light_pz = generateDiagPidControl(G_light_pz.G_cart, fs);
%
% K_heavy_vc = generateDiagPidControl(G_heavy_vc.G_cart, fs);
% K_heavy_pz = generateDiagPidControl(G_heavy_pz.G_cart, fs);
%% Save the MIMO control
save('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');
% save('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');

10
demonstration/displacement_init.m
View File

@ -58,11 +58,11 @@ mass((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts, 1) = mass((T_mas
mass((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts, 2) = mass((T_mass_start+2)/sim_conf.Ts, 2)-2*pi*(-10/360)*(time_vector((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts)-time_vector((T_mass_start+3)/sim_conf.Ts));
opts_inputs = struct(...
'ty', ty, ...
'ry', ry, ...
'rz', rz, ...
'u_hexa', u_hexa, ...
'mass', mass ...
'Dy', ty, ...
'Ry', ry, ...
'Rz', rz, ...
'Dh', u_hexa, ...
'Rm', mass ...
);
initializeInputs(opts_inputs);

8
demonstration/sample_pos_init.m
View File

@ -3,7 +3,7 @@ clear; close all; clc;
%% Initialize simulation configuration
opts_sim = struct(...
'Tsim', 0.1 ...
'Tsim', 0.001 ...
);
initializeSimConf(opts_sim);
@ -14,14 +14,14 @@ load('./mat/sim_conf.mat', 'sim_conf')
time_vector = 0:sim_conf.Ts:sim_conf.Tsim;
% Translation Stage
Ty = 0.20*ones(length(time_vector), 1);
Ty = 0*ones(length(time_vector), 1);
% Tilt Stage
Ry = 2*pi*(3/360)*ones(length(time_vector), 1);
Ry = 2*pi*(0/360)*ones(length(time_vector), 1);
% Ry = 2*pi*(3/360)*sin(2*pi*time_vector);
% Spindle
Rz = 2*pi*3*(time_vector);
Rz = 2*pi*0*(time_vector);
% Rz = 2*pi*(190/360)*ones(length(time_vector), 1);
% Micro Hexapod

BIN
demonstration/sim_nano_station_disp.slx
View File

Binary file not shown.

BIN
identification/sim_micro_station_id.slx
View File

Binary file not shown.

BIN
identification/sim_nano_station_id.slx
View File

Binary file not shown.

12
initialize/initializeExperiment.m
View File

@ -8,18 +8,20 @@ function [] = initializeExperiment(exp_name, sys_mass)
if strcmp(sys_mass, 'light')
opts_inputs = struct(...
'ground_motion', true, ...
'rz', 60 ... % rpm
'Dw', true, ...
'Rz', 60 ... % rpm
);
elseif strcpm(sys_mass, 'heavy')
opts_inputs = struct(...
'ground_motion', true, ...
'rz', 1 ... % rpm
'Dw', true, ...
'Rz', 1 ... % rpm
);
else
error('sys_mass should be light or heavy');
end
initializeInputs(opts_inputs);
elseif
else
error('exp_name is only configured for tomography');
end
end

3
initialize/initializeGranite.m
View File

@ -19,6 +19,9 @@ function [granite] = initializeGranite()
granite.k.z = 1e8; % [N/m]
granite.c.z = 1e4; % [N/(m/s)]
%% Positioning parameters
granite.sample_pos = 0.8; % Z-offset for the initial position of the sample [m]
%% Save
save('./mat/stages.mat', 'granite', '-append');
end

2
initialize/initializeInputs.m
View File

@ -62,7 +62,7 @@ function [inputs] = initializeInputs(opts_param)
elseif islogical(opts.Rz) && opts.Rz == false
Rz = zeros(length(t), 1);
elseif isnumeric(opts.Rz) && length(opts.Rz) == 1
Rz = opts.Rz*(2*pi/360)*ones(length(t), 1);
Rz = opts.Rz*(2*pi/60)*t;
else
Rz = opts.Rz;
end

3
initialize/initializeMicroHexapod.m
View File

@ -12,7 +12,8 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Stewart Object
micro_hexapod = struct();
micro_hexapod.h = 350; % Total height of the platform [mm]
micro_hexapod.jacobian = 265; % Distance from the top platform to the Jacobian point [mm]
% micro_hexapod.jacobian = 269.26; % Distance from the top platform to the Jacobian point [mm]
micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm]
%% Bottom Plate - Mechanical Design
BP = struct();

1
initialize/initializeNanoHexapod.m
View File

@ -13,6 +13,7 @@ function [nano_hexapod] = initializeNanoHexapod(opts_param)
nano_hexapod = struct();
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 = 174.26; % Point where the Jacobian is computed => Center of rotation [mm]
%% Bottom Plate
BP = struct();

3
initialize/initializeRy.m
View File

@ -47,6 +47,9 @@ function [ry] = initializeRy(opts_param)
ry.c.rad = 10*(1/5)*sqrt(ry.k.rad/ry.m);
ry.c.rrad = 10*(1/5)*sqrt(ry.k.rrad/ry.m);
ry.c.tilt = 10*(1/1)*sqrt(ry.k.tilt/ry.m);
%% Positioning parameters
ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m]
%% Save
save('./mat/stages.mat', 'ry', '-append');

BIN
mat/G.mat
View File

Binary file not shown.

BIN
mat/K_fb.mat
View File

Binary file not shown.

BIN
mat/config.mat
View File

Binary file not shown.

BIN
mat/controller.mat
View File

Binary file not shown.

BIN
mat/controllers.mat
View File

Binary file not shown.

BIN
mat/inputs.mat
View File

Binary file not shown.

BIN
mat/sim_conf.mat
View File

Binary file not shown.

BIN
mat/stages.mat
View File

Binary file not shown.

BIN
nass_library/nass_library.slx
View File

Binary file not shown.

BIN
sim_nano_station_ctrl.slx
View File

Binary file not shown.

2
src/generateDiagPidControl.m
View File

@ -7,7 +7,7 @@ function [K] = generateDiagPidControl(G, fs)
%%
K = tf(zeros(6));
for i = 1:5
for i = 1:6
input_name = G.InputName(i);
output_name = G.OutputName(i);
K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));

6
src/identifyPlant.m
View File

@ -24,6 +24,7 @@ function [sys] = identifyPlant(opts_param)
io(5) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
io(6) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
io(7) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
io(8) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
%% Run the linearization
G = linearize(mdl, io, 0);
@ -33,7 +34,8 @@ function [sys] = identifyPlant(opts_param)
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'};
%% Create the sub transfer functions
% From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
@ -46,4 +48,6 @@ function [sys] = identifyPlant(opts_param)
sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
% From forces applied on NASS's legs to displacement of each leg
sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
% From forces applied on NASS's legs to displacement of each leg
sys.G_plant = minreal(G({'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
end

8
src/runSimulation.m
View File

@ -3,14 +3,14 @@ function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
K_obj = load('./mat/K_fb.mat');
K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
K_obj = load('./mat/K_fb_iff.mat');
K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
elseif strcmp(ctrl_type, 'ol')
K = tf(zeros(6)); %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
else
error('ctrl_type should be cl or ol');
end
@ -46,7 +46,7 @@ function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
sim('sim_nano_station_ctrl.slx');
%% Split the Dsample matrix into vectors
[Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Dsample.Data, 1); %#ok
[Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok
time = Dsample.Time; %#ok
%% Save the result