%% This script describe the model of a rotary joint actuated by a linear motor.
%% More details on: https://lucidar.me/en/mechanics/model-of-a-rotary-joint-driven-by-a-linear-motor/

close all;
clear all;
clc;
%% Inline function for drawing circles
drawCircle = @(C,R,color) plot ( C(1) + R*cos([0:0.01:2*pi]) , C(2) + R*sin([0:0.01:2*pi]), color );

% Frame length
OC=200;
% Lever arm
OA=50;
% Motor offset
BC=40;
% Motor lenght
AB=150;

O=[0,0];
C=[0, OC];
motorForce = 67;


plot (O(1), O(2), 'ok');
hold on;
axis square equal;
grid on;

line ([O(1), C(1)], [O(2), C(2)],'color', 'k', 'LineWidth', 2);
plot (C(1), C(2), 'ok');

%% Calcule de alpha et P2
alpha = acos( (OC*OC+OA*OA-AB*AB-BC*BC) / (2*OC*OA) )
A = [ OA*cos(alpha+pi/2) , OA*sin(alpha+pi/2) ];
plot (A(1), A(2), 'ob');
line ([O(1), A(1)], [O(2), A(2)],'color', 'b', 'LineWidth', 2);

%% Draw circles
drawCircle(C, BC, 'k:');
drawCircle(A, AB, 'k:');


%% Compute coordinates of point B
d = sqrt( BC*BC + AB*AB );
a = (AB*AB - BC*BC + d*d)/(2*d);
h = sqrt(AB*AB - a*a);
x1 = A(1) + (a/d) *( C(1)-A(1) );
y1 = A(2) + (a/d) *( C(2)-A(2) );

B1 = [ x1 - (h/d) * ( C(2)-A(2) ) , y1 + (h/d) * ( C(1)-A(1) ) ];
B2 = [ x1 + (h/d) * ( C(2)-A(2) ) , y1 - (h/d) * ( C(1)-A(1) ) ];

%% Display point B (option 1)
plot (B1(1), B1(2), 'x');
line ([A(1), B1(1)], [A(2), B1(2)],'color', 'g', 'LineWidth', 2);
line ([C(1), B1(1)], [C(2), B1(2)],'color', 'r', 'LineWidth', 3);

%% Display point B (option 1)
plot (B2(1), B2(2), 'x');
line ([A(1), B2(1)], [A(2), B2(2)],'color', 'g', 'LineWidth', 2, 'LineStyle', ':');
line ([C(1), B2(1)], [C(2), B2(2)],'color', 'r', 'LineWidth', 2, 'LineStyle', ':');


%% Force vector
F = motorForce * (A-B1)/norm(A-B1);

%% Torque (divide by 1000 to get Nm)
Torque = ( F(1)*A(2) - F(2)*A(1) ) /1000