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

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);

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

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');

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);

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

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

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

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

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();

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();

initialize/initializeRy.m

@@ -48,6 +48,9 @@ function [ry] = initializeRy(opts_param)
     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');
 end

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));

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

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