99 lines
1.6 KiB
Mathematica
99 lines
1.6 KiB
Mathematica
|
%% Clear Workspace and Close figures
|
||
|
|
clear; close all; clc;
|
||
|
|
|
||
|
|
%% Intialize Laplace variable
|
||
|
|
s = zpk('s');
|
||
|
|
|
||
|
|
% Simulink
|
||
|
|
|
||
|
|
open 'gravity_test.slx'
|
||
|
|
|
||
|
|
% Initial Simulation
|
||
|
|
|
||
|
|
g = -10; % [m/s^2]
|
||
|
|
|
||
|
|
k = 1e3; % Stiffness [N/m]
|
||
|
|
c = 10; % Damping [N/(m/s)]
|
||
|
|
|
||
|
|
l0 = 0; % Initial wanted position [m]
|
||
|
|
leq = 0; % equilibrium position [m]
|
||
|
|
|
||
|
|
F_ext = 0; % External force [N]
|
||
|
|
F_act = 0; % Actuator force [N]
|
||
|
|
|
||
|
|
out_init = sim('gravity_test.slx')
|
||
|
|
|
||
|
|
out_init.d.Name = 'Displacement';
|
||
|
|
out_init.Fm.Name = 'Force Sensor';
|
||
|
|
|
||
|
|
figure;
|
||
|
|
subplot(1,2,1)
|
||
|
|
plot(out_init.d)
|
||
|
|
title('');
|
||
|
|
subplot(1,2,2)
|
||
|
|
plot(out_init.Fm)
|
||
|
|
title('');
|
||
|
|
|
||
|
|
% Change the equilibrium position
|
||
|
|
|
||
|
|
l0 = 0; % Initial wanted position [m]
|
||
|
|
leq = -out_init.Fm.Data(end)/k; % equilibrium position [m]
|
||
|
|
|
||
|
|
F_ext = 0; % External force [N]
|
||
|
|
F_act = 0; % Actuator force [N]
|
||
|
|
|
||
|
|
out = sim('gravity_test.slx');
|
||
|
|
|
||
|
|
out.d.Name = 'Displacement';
|
||
|
|
out.Fm.Name = 'Force Sensor';
|
||
|
|
|
||
|
|
figure;
|
||
|
|
subplot(1,2,1)
|
||
|
|
plot(out.d)
|
||
|
|
title('');
|
||
|
|
subplot(1,2,2)
|
||
|
|
plot(out.Fm)
|
||
|
|
title('');
|
||
|
|
|
||
|
|
% Add external force
|
||
|
|
|
||
|
|
l0 = 0; % Initial wanted position [m]
|
||
|
|
leq = 0; % equilibrium position [m]
|
||
|
|
|
||
|
|
F_ext = -out_init.Fm.Data(end); % External force [N]
|
||
|
|
F_act = 0; % Actuator force [N]
|
||
|
|
|
||
|
|
out = sim('gravity_test.slx');
|
||
|
|
|
||
|
|
out.d.Name = 'Displacement';
|
||
|
|
out.Fm.Name = 'Force Sensor';
|
||
|
|
|
||
|
|
figure;
|
||
|
|
subplot(1,2,1)
|
||
|
|
plot(out.d)
|
||
|
|
title('');
|
||
|
|
subplot(1,2,2)
|
||
|
|
plot(out.Fm)
|
||
|
|
title('');
|
||
|
|
|
||
|
|
% Change initial position
|
||
|
|
|
||
|
|
l0 = out_init.d.Data(end); % Initial wanted position [m]
|
||
|
|
leq = 0; % equilibrium position [m]
|
||
|
|
|
||
|
|
F_ext = 0; % External force [N]
|
||
|
|
F_act = 0; % Actuator force [N]
|
||
|
|
|
||
|
|
out = sim('gravity_test.slx');
|
||
|
|
|
||
|
|
out.d.Name = 'Displacement';
|
||
|
|
out.Fm.Name = 'Force Sensor';
|
||
|
|
|
||
|
|
figure;
|
||
|
|
subplot(1,2,1)
|
||
|
|
plot(out.d)
|
||
|
|
title('');
|
||
|
|
subplot(1,2,2)
|
||
|
|
plot(out.Fm)
|
||
|
|
title('');
|