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

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

addpath('./mat/');

% Data Loading
% The measured data is loaded and the first 25 seconds of data corresponding to transient data are removed.


load('force_sensor_sin.mat', 't', 'encoder', 'u', 'v');

u       = u(t>25);
v       = v(t>25);
encoder = encoder(t>25) - mean(encoder(t>25));
t       = t(t>25);

% Excitation signal and corresponding displacement
% The driving voltage is a sinus at 0.5Hz centered on 3V and with an amplitude of 3V (Figure [[fig:force_sensor_sin_u]]).


figure;
plot(t, u)
xlabel('Time [s]'); ylabel('Control Voltage [V]');



% #+name: fig:force_sensor_sin_u
% #+caption: Driving Voltage
% #+RESULTS:
% [[file:figs/force_sensor_sin_u.png]]

% The corresponding displacement as measured by the encoder is shown in Figure [[fig:force_sensor_sin_encoder]].

% The full stroke is:

max(encoder)-min(encoder)



% #+RESULTS:
% : 5.005e-05


figure;
plot(t, encoder)
xlabel('Time [s]'); ylabel('Encoder [m]');

% Generated Voltage
% The generated voltage by the stack is shown in Figure [[fig:force_sensor_sin_stack]].


figure;
plot(t, v)
xlabel('Time [s]'); ylabel('Force Sensor Output [V]');

% Generated Charge
% The capacitance of the stack is

Cp = 4.4e-6; % [F]



% The voltage and charge across a capacitor are related through the following equation:
% \begin{equation}
%   U_C = \frac{Q}{C}
% \end{equation}
% where $U_C$ is the voltage in Volts, $Q$ the charge in Coulombs and $C$ the capacitance in Farads.


% The corresponding generated charge is then shown in Figure [[fig:force_sensor_sin_charge]].


figure;
plot(t, 1e6*Cp*(v-mean(v)))
xlabel('Time [s]'); ylabel('Generated Charge [$\mu C$]');

% Generated Voltage/Charge as a function of the displacement
% The relation between the generated voltage and the measured displacement is almost linear as shown in Figure [[fig:force_sensor_linear_relation]].


b1 = encoder\(v-mean(v));

figure;
hold on;
plot(encoder, v-mean(v), 'DisplayName', 'Measured Voltage');
plot(encoder, encoder*b1, 'DisplayName', sprintf('Linear Fit: $U_s \\approx %.3f [V/\\mu m] \\cdot d$', 1e-6*abs(b1)));
hold off;
xlabel('Measured Displacement [m]'); ylabel('Generated Voltage [V]');
legend();



% #+name: fig:force_sensor_linear_relation
% #+caption: Almost linear relation between the relative displacement and the generated voltage
% #+RESULTS:
% [[file:figs/force_sensor_linear_relation.png]]

% With a 16bits ADC, the resolution will then be equals to (in [nm]):

abs((20/2^16)/(b1/1e9))
