%% 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;

% Flexible joint Geometry
% The flexible joint used for the Nano-Hexapod is shown in Figure ref:fig:test_joints_bend_geometry.
% Its bending stiffness is foreseen to be $k_{R_y}\approx 5\,\frac{Nm}{rad}$ and its stroke $\theta_{y,\text{max}}\approx 25\,mrad$.

% #+name: fig:test_joints_bend_geometry
% #+caption: Geometry of the flexible joint
% [[file:figs/test_joints_bend_geometry.png]]

% The height between the flexible point (center of the joint) and the point where external forces are applied is $h = 20\,mm$.

% Let's define the parameters on Matlab.

kRx = 5; % Bending Stiffness [Nm/rad]
Rxmax = 25e-3; % Bending Stroke [rad]
h = 20e-3; % Height [m]

% Required external applied force

% The bending $\theta_y$ of the flexible joint due to the force $F_x$ is:
% \begin{equation}
%   \theta_y = \frac{M_y}{k_{R_y}} = \frac{F_x h}{k_{R_y}}
% \end{equation}

% Therefore, the applied force to test the full range of the flexible joint is:
% \begin{equation}
%   F_{x,\text{max}} = \frac{k_{R_y} \theta_{y,\text{max}}}{h}
% \end{equation}


Fxmax = kRx*Rxmax/h; % Force to induce maximum stroke [N]



% And we obtain:

sprintf('\\begin{equation} F_{x,max} = %.1f\\, [N] \\end{equation}', Fxmax)

% Required actuator stroke and sensors range

% The flexible joint is designed to allow a bending motion of $\pm 25\,mrad$.
% The corresponding stroke at the location of the force sensor is:
% \[ d_{x,\text{max}} = h \tan(R_{x,\text{max}}) \]


dxmax = h*tan(Rxmax);

sprintf('\\begin{equation} d_{max} = %.1f\\, [mm] \\end{equation}', 1e3*dxmax)