close all;
clear all;
clc;

l_link=0.25; %length of shank/thigh [m]
w_link=0.005; %width of shank/thigh [m]
h_link=0.04; %height of shank/thigh [m]
rho_al=2700;

m_1=1.1015+3/4*(l_link*w_link*h_link*rho_al); %mass on top
m_2=0.001; %mass at the foot
g=9.81; %gravity constant
l_0=0.5; %rest length in m
x1_init=0.2; %initial position of the mass
x2_init=0;
h_goal=0.7; %change of height of the hip goal
omega_set=2*pi;%sqrt(2*g/(l_0-x1_init));
k_s=omega_set^2*m_1;%2*g*m_1/(2*l_0-x1_init-h_goal);%2*m_1*g*h_goal/((l_0-x1_init)^2); %spring stiffness in N/m
d_s=0.00;

k_g=1000000; %spring stiffness ground in N/m
d_g=100; %damping constant ground in Ns/m

StartTime=0;
StopTime=20;

%set_param('extented_model_150602_VMC','FixedStep','0.0001');
set_param('SLIP_150616','StartTime',num2str(StartTime));
set_param('SLIP_150616','StopTime',num2str(StopTime));

sim('SLIP_150616');

figure(6);
% subplot(3,1,1);
plot(x1,'linewidth',2);
hold on;
plot(x2,'linewidth',2);
hold on;
% plot(time,0.2*sin(9.3379*time+3/2*pi));
xlabel('\textbf{time in s}','FontSize',14,'interpreter','latex');
ylabel('\textbf{position of mass in m}','FontSize',14,'interpreter','latex');
title('\textbf{Single Mass Oscillator}','FontSize',14,'interpreter','latex');
ylim([-0.1 0.6]);
grid on;

% subplot(3,1,2);
% plot(F_S,'k','linewidth',2);
% hold on;
% plot(F_grf,'b--','linewidth',2);
% xlabel('time in s');
% ylabel('Force by spring/ GRF in N');
% grid on;
% 
% subplot(3,1,3);
% plot(F_S-F_grf);
% % ylim([-0.01 0.001]);
% xlabel('time in s');
% ylabel('velocity of the foot in m/s');
% grid on;