diff --git a/control/control_cl_sim.m b/control/control_cl_sim.m index 0b32bd1..b21a26e 100644 --- a/control/control_cl_sim.m +++ b/control/control_cl_sim.m @@ -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); diff --git a/control/control_cl_tf.m b/control/control_cl_tf.m index e26af84..3fdda91 100644 --- a/control/control_cl_tf.m +++ b/control/control_cl_tf.m @@ -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 diff --git a/control/control_generate.m b/control/control_generate.m index cd42114..aef54cc 100644 --- a/control/control_generate.m +++ b/control/control_generate.m @@ -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'); diff --git a/demonstration/displacement_init.m b/demonstration/displacement_init.m index 7b125d3..84c4c1a 100644 --- a/demonstration/displacement_init.m +++ b/demonstration/displacement_init.m @@ -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); diff --git a/demonstration/sample_pos_init.m b/demonstration/sample_pos_init.m index cb8ffb9..a4d1bab 100644 --- a/demonstration/sample_pos_init.m +++ b/demonstration/sample_pos_init.m @@ -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 diff --git a/demonstration/sim_nano_station_disp.slx b/demonstration/sim_nano_station_disp.slx index b482687..395d123 100644 Binary files a/demonstration/sim_nano_station_disp.slx and b/demonstration/sim_nano_station_disp.slx differ diff --git a/identification/sim_micro_station_id.slx b/identification/sim_micro_station_id.slx index 992b0d0..19788e1 100644 Binary files a/identification/sim_micro_station_id.slx and b/identification/sim_micro_station_id.slx differ diff --git a/identification/sim_nano_station_id.slx b/identification/sim_nano_station_id.slx index b3ba49b..2dbea4e 100644 Binary files a/identification/sim_nano_station_id.slx and b/identification/sim_nano_station_id.slx differ diff --git a/initialize/initializeExperiment.m b/initialize/initializeExperiment.m index 88f47b3..4095832 100644 --- a/initialize/initializeExperiment.m +++ b/initialize/initializeExperiment.m @@ -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 diff --git a/initialize/initializeGranite.m b/initialize/initializeGranite.m index 2bc12ac..d47b17c 100644 --- a/initialize/initializeGranite.m +++ b/initialize/initializeGranite.m @@ -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 diff --git a/initialize/initializeInputs.m b/initialize/initializeInputs.m index 3ac5d41..66e50e7 100644 --- a/initialize/initializeInputs.m +++ b/initialize/initializeInputs.m @@ -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 diff --git a/initialize/initializeMicroHexapod.m b/initialize/initializeMicroHexapod.m index 2457810..c6063d9 100644 --- a/initialize/initializeMicroHexapod.m +++ b/initialize/initializeMicroHexapod.m @@ -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(); diff --git a/initialize/initializeNanoHexapod.m b/initialize/initializeNanoHexapod.m index 4429baa..19b084f 100644 --- a/initialize/initializeNanoHexapod.m +++ b/initialize/initializeNanoHexapod.m @@ -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(); diff --git a/initialize/initializeRy.m b/initialize/initializeRy.m index 15e4ca1..71afabf 100644 --- a/initialize/initializeRy.m +++ b/initialize/initializeRy.m @@ -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'); diff --git a/mat/G.mat b/mat/G.mat index 4afdfff..b2a6ff9 100644 Binary files a/mat/G.mat and b/mat/G.mat differ diff --git a/mat/K_fb.mat b/mat/K_fb.mat index f4e6db6..d08aaed 100644 Binary files a/mat/K_fb.mat and b/mat/K_fb.mat differ diff --git a/mat/config.mat b/mat/config.mat index a8de8e8..fb46783 100644 Binary files a/mat/config.mat and b/mat/config.mat differ diff --git a/mat/controller.mat b/mat/controller.mat index ad5fdb4..dfdcf66 100644 Binary files a/mat/controller.mat and b/mat/controller.mat differ diff --git a/mat/controllers.mat b/mat/controllers.mat index 1b87372..28d2766 100644 Binary files a/mat/controllers.mat and b/mat/controllers.mat differ diff --git a/mat/inputs.mat b/mat/inputs.mat index 7ec6143..dcef1fd 100644 Binary files a/mat/inputs.mat and b/mat/inputs.mat differ diff --git a/mat/sim_conf.mat b/mat/sim_conf.mat index 6ea886f..c0878e8 100644 Binary files a/mat/sim_conf.mat and b/mat/sim_conf.mat differ diff --git a/mat/stages.mat b/mat/stages.mat index ae52dc8..561cf2c 100644 Binary files a/mat/stages.mat and b/mat/stages.mat differ diff --git a/nass_library/nass_library.slx b/nass_library/nass_library.slx index e83c73d..83401ff 100644 Binary files a/nass_library/nass_library.slx and b/nass_library/nass_library.slx differ diff --git a/sim_nano_station_ctrl.slx b/sim_nano_station_ctrl.slx index a09874b..36c3181 100644 Binary files a/sim_nano_station_ctrl.slx and b/sim_nano_station_ctrl.slx differ diff --git a/src/generateDiagPidControl.m b/src/generateDiagPidControl.m index ca9b824..47a777a 100644 --- a/src/generateDiagPidControl.m +++ b/src/generateDiagPidControl.m @@ -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)); diff --git a/src/identifyPlant.m b/src/identifyPlant.m index 9d1d4de..640acc4 100644 --- a/src/identifyPlant.m +++ b/src/identifyPlant.m @@ -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 diff --git a/src/runSimulation.m b/src/runSimulation.m index 7d7ffef..16801b7 100644 --- a/src/runSimulation.m +++ b/src/runSimulation.m @@ -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