Update the Rz reference: offset added after filter

This commit is contained in:
Thomas Dehaeze 2020-02-04 10:28:05 +01:00
parent b2ca19bf05
commit cfc4cf28f5
4 changed files with 40 additions and 36 deletions

View File

@ -506,12 +506,11 @@ The Simscape model of the Spindle is composed of:
:END:
#+begin_src matlab
arguments
args.rigid logical {mustBeNumericOrLogical} = false
args.x0 (1,1) double {mustBeNumeric} = 0 % [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % [m]
args.rx0 (1,1) double {mustBeNumeric} = 0 % [rad]
args.ry0 (1,1) double {mustBeNumeric} = 0 % [rad]
args.x0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
end
#+end_src
@ -1199,29 +1198,32 @@ The =sample= structure is saved.
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Spindle - Rz
t = 0:Ts:Tmax; % Time Vector [s]
Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1);
%% 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'
Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t;
switch args.Rz_type
case 'constant'
Rz(:) = args.Rz_amplitude;
Rzd(:) = 0;
Rzdd(:) = 0;
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);
otherwise
warning('Rz_type is not set correctly');
end
% 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);
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
% 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);
#+end_src
** Micro Hexapod

Binary file not shown.

View File

@ -118,16 +118,19 @@ Rzdd = zeros(length(t), 1);
switch args.Rz_type
case 'constant'
Rz(:) = args.Rz_amplitude;
Rzd(:) = 0;
Rzdd(:) = 0;
Rz(:) = args.Rz_amplitude;
Rzd(:) = 0;
Rzdd(:) = 0;
case 'rotating'
Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t;
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

View File

@ -1,12 +1,11 @@
function [rz] = initializeRz(args)
arguments
args.rigid logical {mustBeNumericOrLogical} = false
args.x0 (1,1) double {mustBeNumeric} = 0 % [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % [m]
args.rx0 (1,1) double {mustBeNumeric} = 0 % [rad]
args.ry0 (1,1) double {mustBeNumeric} = 0 % [rad]
args.x0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
end
rz = struct();