% Control3_3.m
% Solution to problem 3 of control 3
% J.M.Soler, May 2019

% Set parameters
clear
mass = 5e-3;                      % particle mass (kg)
km = [4.5,1.5,2.3];               % spring constants (N/m)
lm = [1.2,0.8,0.6];               % equilibrium spring lengths (m)
rm = [0,0.6,-0.3; 0.8,-0.5,-0.5]; % spring base positions (m)
mu = 0.025;                       % friction coefficient
x0 = 0;                           % initial position (m)
E0 = 0.6;                         % inital mass energy (m)
dt = 5e-4;                        % simulation time step (s)
tmax = 2;                         % max. simulation time (s)

% Find initial velocity
[~,Ep0] = Force_Potential_3_springs(x0,rm,km,lm);
Ek0 = E0-Ep0;
v0 = sqrt(2*Ek0/mass);

% Integrate equation of motion
myforce = @(x,v,t)totalForce(x, v, rm, km, lm, mu);
[x,v] = rungeKutta( myforce, mass, x0, v0, dt, tmax );
t = 0:dt:tmax;
nt = numel(t);
x = reshape(x,1,nt);
v = reshape(v,1,nt);

% Plot position and velocity
figure(3)
subplot(2,1,1)
plot(t,x)
grid on
ylabel('x (m)')
subplot(2,1,2)
plot(t,v)
grid on
xlabel('time (s)')
ylabel('v (m/s)')

% Find work of friction
[Fel,Ep] = Force_Potential_3_springs(x, rm, km, lm);
Ff = -mu*abs(Fel(2,:)).*sign(v);
Wf = integral(t,Ff.*v);

% Plot energies
Ek = 0.5*mass*v.^2;
Et = Ek+Ep;
figure(4)
plot(t,Et,t,Wf,t,Et-Wf)
legend('E_t','W_f','E_t-Wf')
xlabel('time (s)')
ylabel('Energy (J)')
grid on

