104 lines
2.4 KiB
Matlab
104 lines
2.4 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Add the getAsynchronousError to path
|
|
addpath('./src/');
|
|
|
|
% Parameters
|
|
|
|
mg = 3000; % Mass of granite [kg]
|
|
ms = 50; % Mass of Spindle [kg]
|
|
|
|
kg = 1e8; % Stiffness of granite [N/m]
|
|
ks = 5e7; % Stiffness of spindle [N/m]
|
|
|
|
% Compute Mass and Stiffness Matrices
|
|
|
|
Mm = diag([ms, mg]);
|
|
Km = diag([ks, ks+kg]) - diag(ks, -1) - diag(ks, 1);
|
|
|
|
% Compute resonance frequencies
|
|
|
|
A = [zeros(size(Mm)) eye(size(Mm)) ; -Mm\Km zeros(size(Mm))];
|
|
eigA = imag(eigs(A))/2/pi;
|
|
eigA = eigA(eigA>0);
|
|
eigA = eigA(1:2);
|
|
|
|
% From model_damping compute the Damping Matrix
|
|
|
|
modal_damping = 1e-5;
|
|
|
|
ab = [0.5*eigA(1) 0.5/eigA(1) ; 0.5*eigA(2) 0.5/eigA(2)]\[modal_damping ; modal_damping];
|
|
|
|
Cm = ab(1)*Mm +ab(2)*Km;
|
|
|
|
% Define inputs, outputs and state names
|
|
|
|
StateName = {...
|
|
'xs', ... % Displacement of Spindle [m]
|
|
'xg', ... % Displacement of Granite [m]
|
|
'vs', ... % Velocity of Spindle [m]
|
|
'vg', ... % Velocity of Granite [m]
|
|
};
|
|
StateUnit = {'m', 'm', 'm/s', 'm/s'};
|
|
|
|
InputName = {...
|
|
'f' ... % Spindle Force [N]
|
|
};
|
|
InputUnit = {'N'};
|
|
|
|
OutputName = {...
|
|
'd' ... % Displacement [m]
|
|
};
|
|
OutputUnit = {'m'};
|
|
|
|
% Define A, B and C matrices
|
|
|
|
% A Matrix
|
|
A = [zeros(size(Mm)) eye(size(Mm)) ; ...
|
|
-Mm\Km -Mm\Cm];
|
|
|
|
% B Matrix
|
|
B_low = zeros(length(StateName), length(InputName));
|
|
B_low(strcmp(StateName,'vs'), strcmp(InputName,'f')) = 1;
|
|
B_low(strcmp(StateName,'vg'), strcmp(InputName,'f')) = -1;
|
|
B = blkdiag(zeros(length(StateName)/2), pinv(Mm))*B_low;
|
|
|
|
% C Matrix
|
|
C = zeros(length(OutputName), length(StateName));
|
|
C(strcmp(OutputName,'d'), strcmp(StateName,'xs')) = 1;
|
|
C(strcmp(OutputName,'d'), strcmp(StateName,'xg')) = -1;
|
|
|
|
% D Matrix
|
|
D = zeros(length(OutputName), length(InputName));
|
|
|
|
% Generate the State Space Model
|
|
|
|
sys = ss(A, B, C, D);
|
|
sys.StateName = StateName;
|
|
sys.StateUnit = StateUnit;
|
|
|
|
sys.InputName = InputName;
|
|
sys.InputUnit = InputUnit;
|
|
|
|
sys.OutputName = OutputName;
|
|
sys.OutputUnit = OutputUnit;
|
|
|
|
% Bode Plot
|
|
% The transfer function from a disturbance force $f$ to the measured displacement $d$ is shown figure [[fig:spindle_f_to_d]].
|
|
|
|
|
|
freqs = logspace(-1, 3, 1000);
|
|
|
|
figure;
|
|
plot(freqs, abs(squeeze(freqresp(sys('d', 'f'), freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
|
|
|
% Save the model
|
|
|
|
save('./mat/spindle_model.mat', 'sys');
|