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

Important change for the implementation of motion input

Browse Source 
Now we provide the first and second derivatives.
This permits to not have any motion error.

Also, many experiments (tomography, ty-scans) are simulated
This commit is contained in:
Thomas Dehaeze
2019-12-17 18:03:21 +01:00
parent 65e246ff4c
commit c6a4e5343e
21 changed files with 876 additions and 396 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
src/initializeReferences.m
View File

@@ -1,22 +1,23 @@
function [ref] = initializeReferences(opts_param)
%% Default values for opts
opts = struct( ...
'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Dy_period', 1, ... % Period of the displacement [s]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [rad]
'Ry_period', 10, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0; pi], ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'Ts', 1e-3, ... % Sampling Frequency [s]
'Tmax', 100, ... % Maximum simulation time [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Dy_period', 1, ... % Period of the displacement [s]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [rad]
'Ry_period', 1, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', zeros(6, 1), ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0; pi], ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', zeros(6,1) ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
%% Populate opts with input parameters
@@ -28,56 +29,100 @@ function [ref] = initializeReferences(opts_param)
%% Set Sampling Time
Ts = opts.Ts;
Tmax = opts.Tmax;
%% Low Pass Filter to filter out the references
s = zpk('s');
w0 = 2*pi*100;
xi = 1;
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
%% Translation stage - Dy
t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s]
Dy = zeros(length(t), 1);
t = 0:Ts:Tmax; % Time Vector [s]
Dy = zeros(length(t), 1);
Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1);
switch opts.Dy_type
case 'constant'
Dy(:) = opts.Dy_amplitude;
Dyd(:) = 0;
Dydd(:) = 0;
case 'triangular'
Dy(:) = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t;
Dy(t<0.75*opts.Dy_period) = 2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period);
Dy(t<0.25*opts.Dy_period) = 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period);
% This is done to unsure that we start with no displacement
Dy_raw = opts.Dy_amplitude*sawtooth(2*pi*t/opts.Dy_period,1/2);
i0 = find(t>=opts.Dy_period/4,1);
Dy(1:end-i0+1) = Dy_raw(i0:end);
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
% The signal is filtered out
Dy = lsim(H_lpf, Dy, t);
Dyd = lsim(H_lpf*s, Dy, t);
Dydd = lsim(H_lpf*s^2, Dy, t);
case 'sinusoidal'
Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
Dyd = opts.Dy_amplitude*2*pi/opts.Dy_period*cos(2*pi/opts.Dy_period*t);
Dydd = -opts.Dy_amplitude*(2*pi/opts.Dy_period)^2*sin(2*pi/opts.Dy_period*t);
otherwise
warning('Dy_type is not set correctly');
end
Dy = struct('time', t, 'signals', struct('values', Dy));
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
%% Tilt Stage - Ry
t = 0:Ts:opts.Ry_period-Ts;
Ry = zeros(length(t), 1);
t = 0:Ts:Tmax; % Time Vector [s]
Ry = zeros(length(t), 1);
Ryd = zeros(length(t), 1);
Rydd = zeros(length(t), 1);
switch opts.Ry_type
case 'constant'
Ry(:) = opts.Ry_amplitude;
Ryd(:) = 0;
Rydd(:) = 0;
case 'triangular'
Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
case 'sinusoidal'
Ry_raw = opts.Ry_amplitude*sawtooth(2*pi*t/opts.Ry_period,1/2);
i0 = find(t>=opts.Ry_period/4,1);
Ry(1:end-i0+1) = Ry_raw(i0:end);
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
% The signal is filtered out
Ry = lsim(H_lpf, Ry, t);
Ryd = lsim(H_lpf*s, Ry, t);
Rydd = lsim(H_lpf*s^2, Ry, t);
case 'sinusoidal'
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
Ryd = opts.Ry_amplitude*2*pi/opts.Ry_period*cos(2*pi/opts.Ry_period*t);
Rydd = -opts.Ry_amplitude*(2*pi/opts.Ry_period)^2*sin(2*pi/opts.Ry_period*t);
otherwise
warning('Ry_type is not set correctly');
end
Ry = struct('time', t, 'signals', struct('values', Ry));
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
%% Spindle - Rz
t = 0:Ts:100*opts.Rz_period-Ts;
Rz = zeros(length(t), 1);
t = 0:Ts:Tmax; % Time Vector [s]
Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1);
switch opts.Rz_type
case 'constant'
Rz(:) = opts.Rz_amplitude;
Rzd(:) = 0;
Rzdd(:) = 0;
case 'rotating'
Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
% The signal is filtered out
Rz = lsim(H_lpf, Rz, t);
Rzd = lsim(H_lpf*s, Rz, t);
Rzdd = lsim(H_lpf*s^2, Rz, t);
otherwise
warning('Rz_type is not set correctly');
end
Rz = struct('time', t, 'signals', struct('values', Rz));
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
%% Micro-Hexapod
t = [0, Ts];
@@ -111,11 +156,13 @@ function [ref] = initializeReferences(opts_param)
otherwise
warning('Dh_type is not set correctly');
end
Dh = struct('time', t, 'signals', struct('values', Dh));
Dhl = struct('time', t, 'signals', struct('values', Dhl));
%% Axis Compensation - Rm
t = [0, Ts];
Rm = [opts.Rm_pos, opts.Rm_pos];
Rm = struct('time', t, 'signals', struct('values', Rm));
@@ -129,6 +176,7 @@ function [ref] = initializeReferences(opts_param)
otherwise
warning('Dn_type is not set correctly');
end
Dn = struct('time', t, 'signals', struct('values', Dn));
%% Save