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

% Set parameters
clear
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
xmin = -1;                        % min. mass position (m)
xmax = +1;                        % max. mass position (m)
dxtol = 1e-9;                     % tolerance in equil. mass position (m)
dx = 1e-2;                        % position step for plot (m)

% Find elastic energy as a function of position
x = xmin:dx:xmax;
[Fel,Ep] = Force_Potential_3_springs(x, rm, km, lm);

% Find point of unstable equilibrium
fx = @(f)f(1);
myfunc = @(x)fx(Force_Potential_3_springs(x,rm,km,lm));
xeq = bisectionRoot( myfunc, -0.2, 0.2, dxtol )
[~,Eeq] = Force_Potential_3_springs(xeq,rm,km,lm);

% Plot potential energy
figure(1)
plot(x,Ep,xeq,Eeq,'ro')
grid on
xlabel('x (m)')
ylabel('E_p (J)')

% Plot restoring and friction forces
figure(2)
subplot(2,1,1)
plot(x,Fel(1,:))
grid on
xlabel('x (m)')
ylabel('F_r (N)')
subplot(2,1,2)
plot(x,mu*abs(Fel(2,:)))
grid on
xlabel('x (m)')
ylabel('|F_f| (N)')
