%% 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('');