% mechanism with epicyclic gears
% velocity analysis

clear; clc; close all

AB = 0.050; % (m) AB is link 1
% gear with the center at C is link 2
% gear with the center at E is link 3
% FCDE is link 4
CF = 0.070; % (m) link 4
CD = 0.050; % (m) link 4
DE = 0.025; % (m) link 4
CB = 0.030; % (m) 

% A=O origin
% coordinates of fixed joint F
xF = AB; 
yF = -0.0632;
rF_ = [xF yF 0]; % position vector of F

w1 = 5; % (rad/s) angular velocity of link 1

theta = pi/6; % (rad) angle of 1 with x-axis

% position of B
xB = AB*cos(theta); 
yB = AB*sin(theta); 
rB_ = [xB yB 0]; % position vector of B


% joint C
syms xCsol yCsol
eqnC1 = (xCsol - xB )^2 + ( yCsol - yB )^2 - CB^2;
eqnC2 = (xCsol - xF )^2 + ( yCsol - yF )^2 - CF^2;
solC_ = solve(eqnC1, eqnC2, xCsol, yCsol);
xCpositions = eval(solC_.xCsol);
yCpositions = eval(solC_.yCsol);
xC1 = xCpositions(1);
xC2 = xCpositions(2);
yC1 = yCpositions(1); 
yC2 = yCpositions(2); 
% select the correct position for C 
% for the given input angle theta
if xC1 > xF
    xC = xC1; yC=yC1;
else
    xC = xC2; yC=yC2;
end
rC_ = [xC yC 0]; % position vector of C 

% angle of link 4 with x-axis
phi = atan((yF-yC)/(xF-xC));

xD = xC+CD*cos(phi);
yD = yC+CD*sin(phi);
rD_ = [xD yD 0]; % position vector of C 

xE = xD+DE*cos(phi);
yE = yD+DE*sin(phi);
rE_ = [xE yE 0]; % position vector of C 

fprintf('rF_ =  [ %6.3f, %6.3f, %g ] (m)\n', rF_)
fprintf('rB_ =  [ %6.3f, %6.3f, %g ] (m)\n', rB_)
fprintf('rC_ =  [ %6.3f, %6.3f, %g ] (m)\n', rC_)
fprintf('phi = %6.3f (degrees) \n', phi*180/pi)
fprintf('rD_ =  [ %6.3f, %6.3f, %g ] (m)\n', rD_)
fprintf('rE_ =  [ %6.3f, %6.3f, %g ] (m)\n', rE_)
fprintf('\n')

% rF_ =  [  0.050, -0.063, 0 ] (m)
% rB_ =  [  0.043,  0.025, 0 ] (m)
% rC_ =  [  0.066,  0.005, 0 ] (m)
% phi = 77.055 (degrees) 
% rD_ =  [  0.077,  0.054, 0 ] (m)
% rE_ =  [  0.082,  0.078, 0 ] (m)

r2 = CD; % pitch radius of gear 2
r3 = DE; % pitch radius of gear 3

figure(1)

axis equal
axis square
grid on
hold on

xlabel('x(m)'), ylabel('y(m)')
af = 0.15;
axis([-af af -af af])

plot([0,xB],[0,yB],'k-o','LineWidth',2) 
plot([xB,xC],[yB,yC],'k-.o','LineWidth',1) 
plot([xF,xC],[yF,yC],'k-o','LineWidth',2) 
plot([xC,xE],[yC,yE],'k-o','LineWidth',2)
   
t = linspace(0,2*pi);
% plot gear 2 of radius r2 at C
plot(xC+r2*cos(t),yC+r2*sin(t),...
'b-.','LineWidth',2) 
% plot gear 3 of radius r3 at E
plot(xE+r3*cos(t),yE+r3*sin(t),...
'k-.','LineWidth',2) 
% plot circle of radius CB at C
plot(xC+CB*cos(t),yC+CB*sin(t),...
'k-','LineWidth',1) 

pin_scale_factor = 0.005;             
% plotting joint A
plot_pin0(0,0, pin_scale_factor,0)
plot_pin0(xF,yF, pin_scale_factor,0)

text(0-0.02,0+0.0125,' A=0','fontsize',12)
text(xB,yB+0.01,'  B','fontsize',12)
text(xC,yC,'  C','fontsize',12)
text(xD,yD,'  D','fontsize',12)
text(xE,yE,'  E','fontsize',12)
text(xF,yF,'  F','fontsize',12)

% classical method - velocity analysis

w1_ = [0 0 w1]; % angular velocity of link 1
vA_ = [0 0  0]; % velocity of A (fixed)

% velocity of B (A and B on the same body 1)
vB_ = vA_ + cross(w1_, rB_);
fprintf('vB_ =  [ %6.3f, %6.3f, %g ] (m/s)\n', vB_)
fprintf('\n')

syms omega2 omega4
omega2_ = [0 0 omega2]; % angular velocity of gear 2
omega4_ = [0 0 omega4]; % angular velocity of link 4
vF_ = [0 0 0]; % velocity of F (fixed)

% vC2_ is the velocity of C on gear 2 (C=C2)
vC2_ =  vB_ + cross(omega2_, rC_-rB_);
% vC4_ is the velocity of C on link 4 (C=C4)
vC4_ =  vF_ + cross(omega4_, rC_-rF_);
% C is a revolute joint => vC2_ = vC4_
eqvC_ = vC2_ - vC4_;
eqvCx  = eqvC_(1); % (1)
eqvCy  = eqvC_(2); % (2)
% Eqs.(1)(2) =>
solvC = solve(eqvCx, eqvCy);
omega2  = eval(solvC.omega2);
omega4  = eval(solvC.omega4);
fprintf('omega2 = %6.3f (rad/s)\n',omega2);
fprintf('omega4 = %6.3f (rad/s)\n',omega4);
omega2_ = [0 0 omega2];
omega4_ = [0 0 omega4];

syms omega3 
omega3_ = [0 0 omega3];  % angular velocity of gear 3
% vE4_ is the velocity of E on link 4 (C=C4)
vE4_ = vF_  + cross(omega4_, rE_-rF_);
% E is a revolute joint => vE3_ = vE4_
vE3_ = vE4_;
% vD3_ is the velocity of D on gear 3 (D=D3)
vD3_ = vE3_ + cross(omega3_, rD_-rE_);
% vC2_ is the velocity of C on gear 2 (C=C2)
vC2_ = vB_  + cross(omega2_, rC_-rB_);
% vD2_ is the velocity of D on gear 2 (D=D2)
vD2_ = vC2_ + cross(omega2_, rD_-rC_);
% vD2_ = vD3_
eqvD_ = vD2_ - vD3_;
eqvDx  = eqvD_(1); % (3)
eqvDy  = eqvD_(2);
% Eq.(3)=>
omega3 = eval(solve(eqvDx));
fprintf('omega3 = %6.3f (rad/s)\n',omega3);
fprintf('\n')

% contour method

% contour I
% 0->A->1->B->2->C->4->F->0 
% relative angular velocities

syms w21 w42 w04
w10_ = [0, 0, w1 ];
% w21 is the relative angular velocity of 2 with respect to 1 at B
w21_ = [0, 0, w21];
% w42 is the relative angular velocity of 4 with respect to 2 at C
w42_ = [0, 0, w42];
% w04 is the relative angular velocity of 0 with respect to 4 at F
w04_ = [0, 0, w04];

eq11_ = w10_ + w21_ + w42_ + w04_;
eq11z = eq11_(3); % (4)
eq12_ = cross(rB_, w21_) + cross(rC_, w42_) + cross(rF_, w04_);
eq12x = eq12_(1); % (5)
eq12y = eq12_(2); % (6)

% Eqs.(4)(5)(6)=>
sol1 = solve(eq11z, eq12x, eq12y);
w21 = eval(sol1.w21);
w42 = eval(sol1.w42);
w04 = eval(sol1.w04);

w40 = -w04;     % angular velocity of link 4
w20 = w1 + w21; % angular velocity of gear 2

% w21 = -11.962 (rad/s)
% w42 = 10.833 (rad/s)
% w40 =  3.871 (rad/s)
% w20 = -6.962 (rad/s)

fprintf('w21 = %6.3f (rad/s)\n',w21)  
fprintf('w42 = %6.3f (rad/s)\n',w42) 
fprintf('w40 = %6.3f (rad/s)\n',w40)
fprintf('w20 = %6.3f (rad/s)\n',w20)
fprintf('\n')

% contour II
% 2->C->4->E->3->D->2 
% relative velocities

syms w34 w23
w42_ = [0, 0, w42];
% w34 is the relative angular velocity of 3 with respect to 4 at E
w34_ = [0, 0, w34];
% w23 is the relative angular velocity of 2 with respect to 3 at D
w23_ = [0, 0, w23];

eq21_ = w42_ + w34_ + w23_;
eq21z = eq21_(3); % (7)
eq22_ = cross(rC_, w42_) + cross(rE_, w34_) + cross(rD_, w23_);
eq22x = eq22_(1); % (8)
eq22y = eq22_(2); % (9)

% Eqs.(7)(8)=>
sol2 = solve(eq21z, eq22x);
w34 = eval(sol2.w34);
w23 = eval(sol2.w23);

w30 = w40 + w34;
w3 = w20 - w23;  % angular velocity of gear 3

fprintf('w23 = %6.3f (rad/s)\n',w23)  
fprintf('w34 = %6.3f (rad/s)\n',w34) 
fprintf('w30 = %6.3f (rad/s)\n',w30)

% w23 = -32.499 (rad/s)
% w34 = 21.666 (rad/s)
% w30 = 25.537 (rad/s)

