Rename inputs, add pictures to nass_library

initialize/initializeInputs.m

@@ -1,13 +1,13 @@
 function [inputs] = initializeInputs(opts_param)
     %% Default values for opts
-    opts = struct('setpoint', false, ...
-                  'Dw',       false, ...
-                  'ty',       false, ...
-                  'ry',       false, ...
-                  'Rz',       false, ...
-                  'u_hexa',   false, ...
-                  'mass',     false, ...
-                  'n_hexa',   false  ...
+    opts = struct(   ...
+        'Dw', false, ...
+        'Dy', false, ...
+        'Ry', false, ...
+        'Rz', false, ...
+        'Dh', false, ...
+        'Rm', false, ...
+        'Dn', false  ...
     );
 
     %% Populate opts with input parameters
@@ -21,106 +21,153 @@ function [inputs] = initializeInputs(opts_param)
     load('./mat/sim_conf.mat', 'sim_conf');
 
     %% Define the time vector
-    time_vector = 0:sim_conf.Ts:sim_conf.Tsim;
+    t = 0:sim_conf.Ts:sim_conf.Tsim;
 
-    %% Create the input Structure that will contain all the inputs
-    inputs = struct();
-
-    %% Ground motion
+    %% Ground motion - Dw
     if islogical(opts.Dw) && opts.Dw == true
         load('./mat/perturbations.mat', 'Wxg');
-        Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(time_vector), 3);
-        Dw(:, 1) = lsim(Wxg, Dw(:, 1), time_vector);
-        Dw(:, 2) = lsim(Wxg, Dw(:, 2), time_vector);
-        Dw(:, 3) = lsim(Wxg, Dw(:, 3), time_vector);
+        Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3);
+        Dw(:, 1) = lsim(Wxg, Dw(:, 1), t);
+        Dw(:, 2) = lsim(Wxg, Dw(:, 2), t);
+        Dw(:, 3) = lsim(Wxg, Dw(:, 3), t);
     elseif islogical(opts.Dw) && opts.Dw == false
-        Dw = zeros(length(time_vector), 3);
+        Dw = zeros(length(t), 3);
     else
         Dw = opts.Dw;
     end
 
-    inputs.Dw = timeseries(Dw, time_vector);
-
-    %% Translation stage [m]
-    if islogical(opts.ty) && opts.ty == true
-        ty = zeros(length(time_vector), 1);
-    elseif islogical(opts.ty) && opts.ty == false
-        ty = zeros(length(time_vector), 1);
+    %% Translation stage - Dy
+    if islogical(opts.Dy) && opts.Dy == true
+        Dy = zeros(length(t), 1);
+    elseif islogical(opts.Dy) && opts.Dy == false
+        Dy = zeros(length(t), 1);
     else
-        ty = opts.ty;
+        Dy = opts.Dy;
     end
 
-    inputs.ty = timeseries(ty, time_vector);
-
-    %% Tilt Stage [rad]
-    if islogical(opts.ry) && opts.ry == true
-        ry = 3*(2*pi/360)*sin(2*pi*0.2*time_vector);
-    elseif islogical(opts.ry) && opts.ry == false
-        ry = zeros(length(time_vector), 1);
+    %% Tilt Stage - Ry
+    if islogical(opts.Ry) && opts.Ry == true
+        Ry = 3*(2*pi/360)*sin(2*pi*0.2*t);
+    elseif islogical(opts.Ry) && opts.Ry == false
+        Ry = zeros(length(t), 1);
     else
-        ry = opts.ry;
+        Ry = opts.Ry;
     end
 
-    inputs.ry = timeseries(ry, time_vector);
-
-    %% Spindle [rad]
-    if islogical(opts.rz) && opts.rz == true
-        rz = 2*pi*0.5*time_vector;
-    elseif islogical(opts.rz) && opts.rz == false
-        rz = zeros(length(time_vector), 1);
-    elseif isnumeric(opts.rz) && length(opts.rz) == 1
-        rz = 2*pi*(opts.rz/60)*time_vector;
+    %% Spindle - Rz
+    if islogical(opts.Rz) && opts.Rz == true
+        Rz = 2*pi*0.5*t;
+    elseif islogical(opts.Rz) && opts.Rz == false
+        Rz = zeros(length(t), 1);
+    elseif isnumeric(opts.Rz) && length(opts.Rz) == 1
+        Rz = 2*pi*(opts.Rz/60)*t;
     else
-        rz = opts.rz;
+        Rz = opts.Rz;
     end
 
-    inputs.rz = timeseries(rz, time_vector);
-
-    %% Micro Hexapod
-    if islogical(opts.u_hexa) && opts.setpoint == true
-        u_hexa = zeros(length(time_vector), 6);
-    elseif islogical(opts.u_hexa) && opts.setpoint == false
-        u_hexa = zeros(length(time_vector), 6);
+    %% Micro Hexapod - Dh
+    if islogical(opts.Dh) && opts.Dh == true
+        Dh = zeros(length(t), 6);
+    elseif islogical(opts.Dh) && opts.Dh == false
+        Dh = zeros(length(t), 6);
     else
-        u_hexa = opts.u_hexa;
+        Dh = opts.Dh;
     end
 
-    inputs.u_hexa = timeseries(u_hexa, time_vector);
-
-    %% Center of gravity compensation
-    if islogical(opts.mass) && opts.setpoint == true
-        axisc = zeros(length(time_vector), 2);
-    elseif islogical(opts.mass) && opts.setpoint == false
-        axisc = zeros(length(time_vector), 2);
-        axisc(:, 2) = pi*ones(length(time_vector), 1);
+    %% Axis Compensation - Rm
+    if islogical(opts.Rm)
+        Rm = zeros(length(t), 2);
+        Rm(:, 2) = pi*ones(length(t), 1);
     else
-        axisc = opts.mass;
+        Rm = opts.Rm;
     end
 
-    inputs.axisc = timeseries(axisc, time_vector);
-
-    %% Nano Hexapod
-    if islogical(opts.n_hexa) && opts.setpoint == true
-        n_hexa = zeros(length(time_vector), 6);
-    elseif islogical(opts.n_hexa) && opts.setpoint == false
-        n_hexa = zeros(length(time_vector), 6);
+    %% Nano Hexapod - Dn
+    if islogical(opts.Dn) && opts.Dn == true
+        Dn = zeros(length(t), 6);
+    elseif islogical(opts.Dn) && opts.Dn == false
+        Dn = zeros(length(t), 6);
     else
-        n_hexa = opts.n_hexa;
+        Dn = opts.Dn;
     end
 
-    inputs.n_hexa = timeseries(n_hexa, time_vector);
-
-    %% Set point [m, rad]
-    if islogical(opts.setpoint) && opts.setpoint == true
-        setpoint = zeros(length(time_vector), 6);
-    elseif islogical(opts.setpoint) && opts.setpoint == false
-        setpoint = zeros(length(time_vector), 6);
-    else
-        setpoint = opts.setpoint;
+    %% Setpoint - Ds
+    Ds = zeros(length(t), 6);
+    for i = 1:length(t)
+        Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i));
     end
 
-    inputs.setpoint = timeseries(setpoint, time_vector);
+    %% External Forces applied on the Granite
+    Fg = zeros(length(t), 3);
+
+    %% External Forces applied on the Sample
+    Fs = zeros(length(t), 3);
+
+    %% Create the input Structure that will contain all the inputs
+    inputs = struct( ...
+        't',  t,     ...
+        'Dw', Dw,    ...
+        'Dy', Dy,    ...
+        'Ry', Ry,    ...
+        'Rz', Rz,    ...
+        'Dh', Dh,    ...
+        'Rm', Rm,    ...
+        'Dn', Dn,    ...
+        'Ds', Ds,    ...
+        'Fg', Fg,    ...
+        'Fs', Fs     ...
+    );
 
     %% Save
     save('./mat/inputs.mat', 'inputs');
+
+    %% Custom Functions
+    function setpoint = computeSetpoint(ty, ry, rz)
+    %%
+    setpoint = zeros(6, 1);
+
+    %% Ty
+    TMTy = [1 0 0 0  ;
+          0 1 0 ty ;
+          0 0 1 0  ;
+          0 0 0 1 ];
+
+    %% Ry
+    TMRy = [ cos(ry) 0 sin(ry) 0 ;
+           0       1 0       0 ;
+          -sin(ry) 0 cos(ry) 0 ;
+          0        0 0       1 ];
+
+    %% Rz
+    TMRz = [cos(rz) -sin(rz) 0 0 ;
+          sin(rz)  cos(rz) 0 0 ;
+          0        0       1 0 ;
+          0        0       0 1 ];
+
+    %% All stages
+    TM = TMTy*TMRy*TMRz;
+
+    [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
+
+    setpoint(1:3) = TM(1:3, 4);
+    setpoint(4:6) = [thetax, thetay, thetaz];
+
+    %% Custom Functions
+    function [thetax, thetay, thetaz] = RM2angle(R)
+        if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+            thetay = -asin(R(3, 1));
+            thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+            thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+        else
+            thetaz = 0;
+            if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+                thetay = pi/2;
+                thetax = thetaz + atan2(R(1, 2), R(1, 3));
+            else
+                thetay = -pi/2;
+                thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+            end
+        end
+    end
+    end
 end