%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data

%% Colors for the figures
colors = colororder;

% Force Sensor Calibration

% #+begin_note
% *Load Cells*:
% - [[file:doc/A700000007147087.pdf][FC2231-0000-0010-L]]
% - [[file:doc/FRE_DS_XFL212R_FR_A3.pdf][XFL212R]]
% #+end_note

% There are both specified to have $\pm 1 \%$ of non-linearity over the full range.

% The XFL212R has a spherical interface while the FC2231 has a flat surface.
% Therefore, we should have a nice point contact when using the two force sensors as shown in Figure ref:fig:test_joints_force_sensor_calib.

% #+name: fig:test_joints_force_sensor_calib
% #+caption: Zoom on the two force sensors in contact
% #+attr_latex: :width 0.8\linewidth
% [[file:figs/test_joints_force_sensor_calib.jpg]]

% The two force sensors are therefore measuring the exact same force, and we can compare the two measurements.

% Let's load the measured force of both sensors.

%% Load measurement data
load('calibration_force_sensor.mat', 't', 'F', 'Fc')



% We remove any offset such that they are both measuring no force when not in contact.

%% Remove offset
F  = F  - mean(F( t > 0.5 & t < 1.0));
Fc = Fc - mean(Fc(t > 0.5 & t < 1.0));

figure;
hold on;
plot(t, F, 'DisplayName', 'FC2231');
plot(t, Fc, 'DisplayName', 'XFL212R');
hold off;
xlabel('Time [s]'); ylabel('Measured Force [N]');
xlim([0,15]); ylim([0,55]);
legend('location', 'southeast');



% #+name: fig:test_joints_force_sensor_calib_time
% #+caption: Measured force using both sensors as a function of time
% #+RESULTS:
% [[file:figs/test_joints_force_sensor_calib_time.png]]

% Let's select only the first part from the moment they are in contact until the maximum force is reached.


%% Only get the first part until maximum force
F  = F( t > 1.55 & t < 4.65);
Fc = Fc(t > 1.55 & t < 4.65);



% Then, let's make a linear fit between the two measured forces.


%% Make a line fit
fit_F = polyfit(Fc, F, 1);



% The two forces are plotted against each other as well as the linear fit in Figure ref:fig:test_joints_force_sensor_calib_fit.


figure;
hold on;
plot(Fc, F, '-', 'DisplayName', 'Raw Data');
plot(Fc([1,end]), Fc([1,end])*fit_F(1) + fit_F(2), '--', 'DisplayName', 'Line Fit');
hold off;
xlabel('XFL212R [N]'); ylabel('FC2231 [N]');
xlim([0,50]); ylim([0,50]);
legend('location', 'southeast');



% #+name: fig:test_joints_force_sensor_calib_fit
% #+caption: Measured two forces and linear fit
% #+RESULTS:
% [[file:figs/test_joints_force_sensor_calib_fit.png]]

% The measurement error between the two sensors is shown in Figure ref:fig:test_joints_force_sensor_calib_error.
% It is below 0.1N for the full measurement range.


figure;
hold on;
plot(Fc, F - (Fc*fit_F(1) + fit_F(2)), 'k-');
hold off;
xlim([0,50]); ylim([-0.12, 0.12]);
xlabel('Measured Force [N]');
ylabel('Error [N]')

% Force Sensor Stiffness

% The objective of this measurement is to estimate the stiffness of the force sensor [[file:doc/A700000007147087.pdf][FC2231-0000-0010-L]].

% To do so, a very stiff element is fixed in front of the force sensor as shown in Figure ref:fig:test_joints_meas_force_sensor_stiffness.

% Then, we apply a force on the stiff element through the force sensor.
% We measure the deflection of the force sensor using an encoder.

% Then, having the force and the deflection, we should be able to estimate the stiffness of the force sensor supposing the stiffness of the other elements are much larger.

% #+name: fig:test_joints_meas_force_sensor_stiffness
% #+caption: Bench used to measured the stiffness of the force sensor
% #+attr_latex: :width 0.6\linewidth
% [[file:figs/test_joints_meas_force_sensor_stiffness.jpg]]

% From the documentation, the deflection of the sensor at the maximum load (50N) is 0.05mm, the stiffness is therefore foreseen to be around $1\,N/\mu m$.

% Let's load the measured force as well as the measured displacement.

%% Load measurement data
load('force_sensor_stiffness_meas.mat', 't', 'F', 'd')



% Some pre-processing is applied on the data.

%% Remove offset
F  = F - mean(F(t > 0.5 & t < 1.0));

%% Select important part of data
F  = F( t > 4.55 & t < 7.24);
d  = d( t > 4.55 & t < 7.24); d = d - d(1);
t  = t( t > 4.55 & t < 7.24);



% The linear fit is performed.

%% Linear fit
fit_k = polyfit(F, d, 1);



% The displacement as a function of the force as well as the linear fit are shown in Figure ref:fig:test_joints_force_sensor_stiffness_fit.

figure;
hold on;
plot(F, 1e6*d, '-', 'DisplayName', 'Raw Data');
plot(F([1,end]), 1e6*(F([1,end])*fit_k(1) + fit_k(2)), '--', 'DisplayName', 'Line Fit');
hold off;
xlabel('Force [$N$]'); ylabel('Displacement [$\mu m$]');
xlim([0,45]); ylim([0,60]);
legend('location', 'southeast');



% #+name: fig:test_joints_force_sensor_stiffness_fit
% #+caption: Displacement as a function of the measured force
% #+RESULTS:
% [[file:figs/test_joints_force_sensor_stiffness_fit.png]]

% And we obtain the following stiffness:

%% Force Sensor Stiffness
sprintf('k = %.2f [N/um]', 1e-6*1/fit_k(1));
