242 lines
8.0 KiB
Mathematica
242 lines
8.0 KiB
Mathematica
|
function [ref] = initializeReferences(args)
|
||
|
|
|
||
|
|
arguments
|
||
|
|
% Sampling Frequency [s]
|
||
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
||
|
|
% Maximum simulation time [s]
|
||
|
|
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
|
||
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
||
|
|
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
||
|
|
% Amplitude of the displacement [m]
|
||
|
|
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
|
||
|
|
% Period of the displacement [s]
|
||
|
|
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
||
|
|
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
||
|
|
% Amplitude [rad]
|
||
|
|
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
|
||
|
|
% Period of the displacement [s]
|
||
|
|
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
% Either "constant" / "rotating"
|
||
|
|
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
|
||
|
|
% Initial angle [rad]
|
||
|
|
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
|
||
|
|
% Period of the rotating [s]
|
||
|
|
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
% For now, only constant is implemented
|
||
|
|
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
|
||
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
|
||
|
|
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
|
||
|
|
% For now, only constant is implemented
|
||
|
|
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
|
||
|
|
% Initial position of the two masses
|
||
|
|
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
|
||
|
|
% For now, only constant is implemented
|
||
|
|
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
|
||
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform
|
||
|
|
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Set Sampling Time
|
||
|
|
Ts = args.Ts;
|
||
|
|
Tmax = args.Tmax;
|
||
|
|
|
||
|
|
%% Low Pass Filter to filter out the references
|
||
|
|
s = zpk('s');
|
||
|
|
w0 = 2*pi*10;
|
||
|
|
xi = 1;
|
||
|
|
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
||
|
|
|
||
|
|
%% Translation stage - Dy
|
||
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
||
|
|
Dy = zeros(length(t), 1);
|
||
|
|
Dyd = zeros(length(t), 1);
|
||
|
|
Dydd = zeros(length(t), 1);
|
||
|
|
switch args.Dy_type
|
||
|
|
case 'constant'
|
||
|
|
Dy(:) = args.Dy_amplitude;
|
||
|
|
Dyd(:) = 0;
|
||
|
|
Dydd(:) = 0;
|
||
|
|
case 'triangular'
|
||
|
|
% This is done to unsure that we start with no displacement
|
||
|
|
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
|
||
|
|
i0 = find(t>=args.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(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
|
||
|
|
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
|
||
|
|
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
|
||
|
|
otherwise
|
||
|
|
warning('Dy_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
|
||
|
|
|
||
|
|
%% Tilt Stage - Ry
|
||
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
||
|
|
Ry = zeros(length(t), 1);
|
||
|
|
Ryd = zeros(length(t), 1);
|
||
|
|
Rydd = zeros(length(t), 1);
|
||
|
|
|
||
|
|
switch args.Ry_type
|
||
|
|
case 'constant'
|
||
|
|
Ry(:) = args.Ry_amplitude;
|
||
|
|
Ryd(:) = 0;
|
||
|
|
Rydd(:) = 0;
|
||
|
|
case 'triangular'
|
||
|
|
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
|
||
|
|
i0 = find(t>=args.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(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
|
||
|
|
|
||
|
|
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
|
||
|
|
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
|
||
|
|
otherwise
|
||
|
|
warning('Ry_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
|
||
|
|
|
||
|
|
%% Spindle - Rz
|
||
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
||
|
|
Rz = zeros(length(t), 1);
|
||
|
|
Rzd = zeros(length(t), 1);
|
||
|
|
Rzdd = zeros(length(t), 1);
|
||
|
|
|
||
|
|
switch args.Rz_type
|
||
|
|
case 'constant'
|
||
|
|
Rz(:) = args.Rz_amplitude;
|
||
|
|
Rzd(:) = 0;
|
||
|
|
Rzdd(:) = 0;
|
||
|
|
case 'rotating-not-filtered'
|
||
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
||
|
|
|
||
|
|
% The signal is filtered out
|
||
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
||
|
|
Rzd(:) = 2*pi/args.Rz_period;
|
||
|
|
Rzdd(:) = 0;
|
||
|
|
|
||
|
|
% We add the angle offset
|
||
|
|
Rz = Rz + args.Rz_amplitude;
|
||
|
|
|
||
|
|
case 'rotating'
|
||
|
|
Rz(:) = 2*pi/args.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);
|
||
|
|
|
||
|
|
% We add the angle offset
|
||
|
|
Rz = Rz + args.Rz_amplitude;
|
||
|
|
otherwise
|
||
|
|
warning('Rz_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
|
||
|
|
|
||
|
|
%% Micro-Hexapod
|
||
|
|
t = [0, Ts];
|
||
|
|
Dh = zeros(length(t), 6);
|
||
|
|
Dhl = zeros(length(t), 6);
|
||
|
|
|
||
|
|
switch args.Dh_type
|
||
|
|
case 'constant'
|
||
|
|
Dh = [args.Dh_pos, args.Dh_pos];
|
||
|
|
|
||
|
|
load('nass_stages.mat', 'micro_hexapod');
|
||
|
|
|
||
|
|
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
|
||
|
|
|
||
|
|
tx = args.Dh_pos(4);
|
||
|
|
ty = args.Dh_pos(5);
|
||
|
|
tz = args.Dh_pos(6);
|
||
|
|
|
||
|
|
ARB = [cos(tz) -sin(tz) 0;
|
||
|
|
sin(tz) cos(tz) 0;
|
||
|
|
0 0 1]*...
|
||
|
|
[ cos(ty) 0 sin(ty);
|
||
|
|
0 1 0;
|
||
|
|
-sin(ty) 0 cos(ty)]*...
|
||
|
|
[1 0 0;
|
||
|
|
0 cos(tx) -sin(tx);
|
||
|
|
0 sin(tx) cos(tx)];
|
||
|
|
|
||
|
|
[~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
|
||
|
|
Dhl = [Dhl, Dhl];
|
||
|
|
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 = [args.Rm_pos, args.Rm_pos];
|
||
|
|
Rm = struct('time', t, 'signals', struct('values', Rm));
|
||
|
|
|
||
|
|
%% Nano-Hexapod
|
||
|
|
t = [0, Ts];
|
||
|
|
Dn = zeros(length(t), 6);
|
||
|
|
|
||
|
|
switch args.Dn_type
|
||
|
|
case 'constant'
|
||
|
|
Dn = [args.Dn_pos, args.Dn_pos];
|
||
|
|
|
||
|
|
load('nass_stages.mat', 'nano_hexapod');
|
||
|
|
|
||
|
|
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
|
||
|
|
|
||
|
|
tx = args.Dn_pos(4);
|
||
|
|
ty = args.Dn_pos(5);
|
||
|
|
tz = args.Dn_pos(6);
|
||
|
|
|
||
|
|
ARB = [cos(tz) -sin(tz) 0;
|
||
|
|
sin(tz) cos(tz) 0;
|
||
|
|
0 0 1]*...
|
||
|
|
[ cos(ty) 0 sin(ty);
|
||
|
|
0 1 0;
|
||
|
|
-sin(ty) 0 cos(ty)]*...
|
||
|
|
[1 0 0;
|
||
|
|
0 cos(tx) -sin(tx);
|
||
|
|
0 sin(tx) cos(tx)];
|
||
|
|
|
||
|
|
[~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
|
||
|
|
Dnl = [Dnl, Dnl];
|
||
|
|
otherwise
|
||
|
|
warning('Dn_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Dn = struct('time', t, 'signals', struct('values', Dn));
|
||
|
|
Dnl = struct('time', t, 'signals', struct('values', Dnl));
|
||
|
|
|
||
|
|
micro_hexapod = stewart;
|
||
|
|
if exist('./mat', 'dir')
|
||
|
|
if exist('./mat/nass_references.mat', 'file')
|
||
|
|
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
|
||
|
|
else
|
||
|
|
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
|
||
|
|
end
|
||
|
|
elseif exist('./matlab', 'dir')
|
||
|
|
if exist('./matlab/mat/nass_references.mat', 'file')
|
||
|
|
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts', '-append');
|
||
|
|
else
|
||
|
|
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
|
||
|
|
end
|
||
|
|
end
|