%Script EXO LAMBDA COURS MICRO-372
%Version 2
%Auhtor : Florent Cosandier
%Date: 30.01.2024

close all
clear all

%Geometrical parameters
L = 0.2;                        %[m]
stroke1 = 0.01; %***8***        %[m]
stroke2 = 0.01; %***8***        %[m]
step = 0.005;                   %[m]
alpha0 = 60*pi/180;             %[rad]
beta0 = alpha0;                 %[rad]
gamma0 = pi-beta0-alpha0;       %[rad]
q10 = 0.1;                      %[m]
q20 = q10 + 2*L*cos(alpha0);    %[m]
workspace = 0.04;               %[m]

%Call to the direct geometric model for calculating the initial position of the
%tool
[x0,y0] = lambda_DGM(q10,q20,L);
%Call to inverse geometric model to check the validity of the models
[q10_check, q20_check] = lambda_IGM(x0,y0,L);

%Display of the nominal position in plain lines
lambda_plot_plain(x0,y0,q10,q20)

%Vectors of motor positions
Q1 = q10 + [-stroke1:step:stroke1];
Q2 = q20 + [-stroke2:step:stroke2];

%Empty vectors for joints angles storage 
Alpha = [];
Beta = [];
Gamma = [];

%Loop over the motors positions
for i = 1:length(Q1)
    for j = 1:length(Q2)

        %Current motor positions
        q1 = Q1(i);
        q2 = Q2(j);
        
        %Call to direct geometric model for calculating the current tool
        %position
        [x,y] = lambda_DGM(q1,q2,L);

        %Display of the current structure position in dashed lines
        lambda_plot_dashed(x,y,q1,q2)
        
        %Vectorial calculation of the joint positions as a function of q1, q2, x and y and the positions already known 
        A = ***4***
        B = ***4***
        D = ***4***
        C = ***4***
        
        %Calculation of the joint deflections
        [alpha, beta, gamma] = calculate_angles(A,B,C);

        Alpha = [Alpha; alpha];
        Beta = [Beta; beta];
        Gamma = [Gamma; gamma];
        
        %Check on the angles sum to be equal to pi
        Sum_angles = alpha+beta+gamma;

    end
end

%Display of the workspace
line(1e3*[x0-workspace/2 x0+workspace/2],1e3*[y0+workspace/2 y0+workspace/2],'LineWidth',2.5,'Color','b');
line(1e3*[x0+workspace/2 x0+workspace/2],1e3*[y0-workspace/2 y0+workspace/2],'LineWidth',2.5,'Color','b');
line(1e3*[x0-workspace/2 x0+workspace/2],1e3*[y0-workspace/2 y0-workspace/2],'LineWidth',2.5,'Color','b');
line(1e3*[x0-workspace/2 x0-workspace/2],1e3*[y0-workspace/2 y0+workspace/2],'LineWidth',2.5,'Color','b');

%Display settings, labels and title
line(1e3*[0, 0.4],[0, 0],'LineWidth',1.5,'Color','k');
xlim(1e3*[0, 0.4]);
ylim(1e3*[-0.05, 0.4]);
xlabel('X position [mm]');
ylabel('Y position [mm]');
title('Lambda geometric model');
grid on

%Calculation of the maximal deflection
DefA = max(abs(Alpha-alpha0));
DefB = max(abs(Beta-beta0));
DefC = max(abs(Gamma-gamma0));

DefA_deg = 180*DefA/pi;
DefB_deg = 180*DefB/pi;
DefC_deg = 180*DefC/pi;

%Flexure sizing input parameters
E = 200e9;          %[Pa]
sigma_adm = 900e6;  %[Pa]
b = 10e-3;          %[m]
l_stage = 0.08;     %[m]
l_pivot = 0.014;    %[m]

%Linear and angular stroke calculation
stroke_stage = max(stroke1,stroke2);
def_pivot = max([DefA,DefB,DefC]);

%Flexures thickness calcualtion
%h_stage = ***9***
%h_pivot = ***9***

%Settings to calculate the vertical stiffness of the structure
x = x0;
dy = 0.001;%[m]
y = y0+dy;
[q1,q2] = lambda_IGM(x,y,L);
A = ***4***
B = ***4***
D = ***4***
C = ***4***

%Calculation of the joints deflections for a tool displacement of 1 mm in y 
[alpha, beta, gamma] = calculate_angles(A,B,C);

defl_alpha = alpha-alpha0;
defl_beta = beta-beta0;
defl_gamma = gamma-gamma0;

%Stiffness calculation of the flexible linear stages and separated crossed beam pivots  
%K_stage = ***10***
%K_pivot = ***10***

%Calculation of the x and y stiffnesses of the structure
%Kx = ***11***
%Ky = ***11***

%Function of the Direct Geometric Model
function [x,y] = lambda_DGM(q1,q2,L)

    x = ***1***
    y = ***1***

end

%Function of the Inverse Geometric Model
function [q1,q2] = lambda_IGM(x,y,L)

    q1 = ***2***
    q2 = ***2***

end

%Function for the angles calculation
function [alpha, beta, gamma] = calculate_angles(A,B,C)

    u = ***3***
    v = ***3***
    alpha = acos(dot(u,v) / (norm(u)*norm(v)));
    u = ***3***
    v = ***3***
    beta = acos(dot(u,v) / (norm(u)*norm(v)));
    u = ***3***
    v = ***3***
    gamma = acos(dot(u,v) / (norm(u)*norm(v)));

end

%Function for displaying the lamdba in plain lines
function lambda_plot_plain(x,y,q1,q2)

    line(1e3*[x q2],1e3*[y 0],'LineWidth',3,'Color','k');
    hold on;
    line(1e3*[q1 (q1+q2)/2],1e3*[0 y/2],'LineWidth',3,'Color','k');
    plot(1e3*x,1e3*y,'ko','MarkerFaceColor','r')
    plot(1e3*q1,0,'ko','MarkerFaceColor','r')
    plot(1e3*q2,0,'ko','MarkerFaceColor','r')
    axis equal

end

%Function for displaying the lamdba in dashed lines
function lambda_plot_dashed(x,y,q1,q2)

    line(1e3*[x q2],1e3*[y 0],'LineWidth',0.5,'Color','k','LineStyle','--')
    hold on;
    line(1e3*[q1 (q1+q2)/2],1e3*[0 y/2],'LineWidth',0.5,'Color','k','LineStyle','--')
    plot(1e3*x,1e3*y,'ko','MarkerFaceColor','r')
    plot(1e3*q1,0,'ko','MarkerFaceColor','r')
    plot(1e3*q2,0,'ko','MarkerFaceColor','r')

end