% planetary gear train

clear; clc; close all

m = 24; % (mm) module of the gears
m = m*10^-3; % (m) 

N1 = 22; % (teeth) no. of teeth of sun gear 1
N2 = 20; % (teeth) no. of teeth of planet gear 2 
N2p = 35;% (teeth) no. of teeth of planet gear 2' 
N4 = 15; % (teeth) no. of teeth of sun gear 4  
N5 = 16; % (teeth) no. of teeth of planet gear 5 

n3 = 200; % (rpm) angular velocity of ring gear 3
n6 = 150; % (rpm) angular velocity of ring gear 6

w3 = pi*n3/30; % (rad/s)
w6 = pi*n6/30; % (rad/s)

% angular velocities are along x-axis
omega3_ = [w3, 0, 0]; % angular velocity vector of 3
omega6_ = [w6, 0, 0]; % angular velocity vector of 6

r1 = m*N1/2; % (m) pitch radius of sun gear 1
r2 = m*N2/2; % (m) pitch radius of planet gear 2
r2p = m*N2p/2; % (m) pitch radius of planet gear 2'
r3 = r1 + r2 + r2p;  % (m) pitch radius of ring gear 3
r4 = m*N4/2; % (m) pitch radius of planet gear 4
r5 = m*N5/2; % (m) pitch radius of planet gear 5
r6 = r4 + 2*r5; % (m) pitch radius of ring gear 6

N3 = 2*r3/m; % (teeth) no. of teeth of ring gear 3
N6 = 2*r6/m; % (teeth) no. of teeth of ring gear 6


fprintf('r1 = %6.3f (m) \n',r1)
fprintf('r2 = %6.3f (m) \n',r2)
fprintf('r2p = %6.3f (m) \n',r2p)
fprintf('r3 = %6.3f (m) \n',r3)
fprintf('r4 = %6.3f (m) \n',r4)
fprintf('r5 = %6.3f (m) \n',r5)
fprintf('r6 = %6.3f (m) \n',r6)

fprintf('N3 = %g (teeth) \n',N3)
fprintf('N6 = %g (teeth) \n',N6)

fprintf('\n')

% r1 =  0.264 (m) 
% r2 =  0.240 (m) 
% r2p =  0.420 (m) 
% r3 =  0.924 (m) 
% r4 =  0.180 (m) 
% r5 =  0.192 (m) 
% r6 =  0.564 (m) 
% N3 = 77 (teeth) 
% N6 = 47 (teeth) 

% position vectors
syms xC xD xE xF xG xH xJ
% origin of the reference frame 
% position vector of center of gear 1
rA_ = [0, 0, 0];
% position vector of pitch point B (gears 1 and 2)
rB_ = [0, r1, 0];
% position vector of joint C 
rC_ = [xC, r1 + r2, 0];
% position vector of pitch point D (gears 2' and 3)
rD_ = [xD, r1 + r2 + r2p, 0];
% position vector of joint E 
rE_ = [xE, 0, 0];
% position vector of pitch point F (gears 4 and 5)
rF_ = [xF, r4, 0];
% position vector of joint G 
rG_ = [xG, r4 + r5, 0];
% position vector of pitch point H (gears 5 and 6)
rH_ = [xH, r4 + 2*r5, 0];
% position vector of joint J 
rJ_ = [xJ, 0, 0];

% velocity vectors of fixed joints 
vA_ = [0 0 0]; % velocity vector of A
vE_ = [0 0 0]; % velocity vector of E
vJ_ = [0 0 0]; % velocity vector of J

% Classical method

% unknown angular velocities
syms w1 w2 w4 w5
omega1_ = [w1, 0, 0]; % angular velocity of 1
omega2_ = [w2, 0, 0]; % angular velocity of 2
omega4_ = [w4, 0, 0]; % angular velocity of 4
omega5_ = [w5, 0, 0]; % angular velocity of 5

% G and E are on link 3
vG_  = vE_ + cross(omega3_, rG_-rE_);  
% H and J are on link 6
vH6_ = vJ_ + cross(omega6_, rH_-rJ_); % link 6
vH5_ = vH6_;
% G and H are on link 5
vG5_ = vH5_ + cross(omega5_, rG_-rH_); % link 5
% velocity of G on link 5 is equal to velocity of G on link 3
eqw5_= vG5_-vG_; % (1)
% Eq.(1) =>
w5 = eval(solve(eqw5_(3)));
omega5_ = [w5 0 0];
% w5 =  5.563 (rad/s) 

% F and G are on link 5
vF_  = vG_ + cross(omega5_, rF_-rG_); % link 5
% F and E are on link 4
vF4_ = vE_ + cross(omega4_, rF_-rE_); % link 4
% velocity of F on link 5 is equal to velocity of F on link 4
eqw4_= vF4_-vF_; % (2)
% Eq.(2) =>
w4 = eval(solve(eqw4_(3)));
omega4_ = [w4 0 0];
% w4 = 37.350 (rad/s) 

% D and E are on link 3
vD_ = vE_ + cross(omega3_, rD_-rE_); % link 3
% C and E are on link 4
vC_ = vE_ + cross(omega4_, rC_-rE_); % link 4
% D and C are on link 2
vD2_= vC_ + cross(omega2_, rD_-rC_); % link 2
% velocity of D on link 2 is equal to velocity of D on link 3
eqw2_= vD2_ - vD_; % (3)
% Eq.(3) =>
w2 = eval(solve(eqw2_(3))); 
omega2_ = [w2 0 0];
% w2 =  1.257 (rad/s) 

% B and D are on link 2
vB_ = vD_ + cross(omega2_, rB_-rD_); % link 2
% B and A are on link 1
vB1_= vA_ + cross(omega1_, rB_-rA_); % link 1
% velocity of B on link 1 is equal to velocity of B on link 2
eqw1_= vB1_ - vB_; % (4)
% Eq.(4) =>
w1 = eval(solve(eqw1_(3)));
omega1_ = [w1 0 0];
% w1 = 70.162 (rad/s) 


fprintf('w5 = %6.3f (rad/s) \n',w5)
fprintf('w4 = %6.3f (rad/s) \n',w4)
fprintf('w2 = %6.3f (rad/s) \n',w2)
fprintf('w1 = %6.3f (rad/s) \n',w1)
fprintf('\n')

% w5 =  5.563 (rad/s) 
% w4 = 37.350 (rad/s) 
% w2 =  1.257 (rad/s) 
% w1 = 70.162 (rad/s) 


% Countour method

w03 = -w3; % relative angular velocity of 0 with respect to 3
w60 = w6;  % relative angular velocity of 6 with respect to 0

% contour I: 0-A-1-B-2-C-4-E-0 cw
syms w10 w21 w42 w04
omega10_ = [w10, 0, 0]; % relative angular velocity of 1 with respect to 0
omega21_ = [w21, 0, 0]; % relative angular velocity of 2 with respect to 1 
omega42_ = [w42, 0, 0]; % relative angular velocity of 4 with respect to 2 
omega04_ = [w04, 0, 0]; % relative angular velocity of 0 with respect to 4

% omega10_+omega21_+omega42_+omega04_=0_
eq11_ = omega10_ + omega21_ + omega42_ + omega04_;
eq1x = eq11_(1); % (5)
% rB_ x omega21_ + rC_ x omega42_ = 0_ 
eq12_ = cross(rB_, omega21_) + cross(rC_, omega42_);
eq1z = eq12_(3); % (6)

% contour II: 0-A-1-B-2-D-3-E-0 ccw
syms w32 
omega32_ = [w32, 0, 0]; % relative angular velocity of 3 with respect to 2 
omega03_ = [w03, 0, 0]; % relative angular velocity of 0 with respect to 3

% omega10_+omega21_+omega32+omega03=0_ 
eq21_ = omega10_ + omega21_ + omega32_ + omega03_;
eq2x = eq21_(1);  % (7)
% rB_ x omega21_ + rD_ x omega32_ = 0_ 
eq22_ = cross(rB_, omega21_) + cross(rD_, omega32_);
eq2z = eq22_(3);  % (8)

% contour III: 0-J-6-H-5-G-3-E-0 cw
syms  w56 w35
omega60_ = [w60, 0, 0]; % relative angular velocity of 6 with respect to 0 
omega56_ = [w56, 0, 0]; % relative angular velocity of 5 with respect to 6  
omega35_ = [w35, 0, 0]; % relative angular velocity of 3 with respect to 5  

% omega60_+omega56_+omega35_+omega03_=0_ 
eq31_ = omega60_ + omega56_ + omega35_ + omega03_;
eq3x = eq31_(1);  % (9)
% rH_ x omega56_ + rG_ x omega35_ = 0_
eq32_ = cross(rH_, omega56_) + cross(rG_, omega35_);
eq3z = eq32_(3);  % (10)

% contour IIII: 3-D-2-C-4-F-5-G-3 ccw
syms w54
omega23_ = -omega32_; % relative angular velocity of 2 with respect to 3 
omega54_ = [w54, 0, 0]; % relative angular velocity of 5 with respect to 4 

% omega23_+omega42_+omega54_+omega35_=0_ 
eq41_ = omega23_ + omega42_ + omega54_ + omega35_;
eq4x = eq41_(1);   % (11)
% rD_ x omega23_ + rC_ x omega42_ + rF_ x omega54_ + rG_ x omega35_ = 0_
eq42_ = cross(rD_, omega23_) + cross(rC_, omega42_) ...
    +cross(rF_, omega54_) + cross(rG_, omega35_);
eq4z = eq42_(3);  % (12)

% Eqs.(5)(6)(7)(8)(9)(10)(11)(12)=>
sys= [eq1x eq1z eq2x eq2z eq3x eq3z eq4x eq4z];
sol = solve(sys);

w10 = eval(sol.w10);
w21 = eval(sol.w21);
w42 = eval(sol.w42);
w04 = eval(sol.w04);
w32 = eval(sol.w32);
w35 = eval(sol.w35);
w56 = eval(sol.w56);
w54 = eval(sol.w54);

w2 = w10+w21; % angular velocity of 2 
w4 = -w04;    % angular velocity of 4
w5 = w4+w54;  % angular velocity of 5
w2 = w10+w21; % angular velocity of 2

fprintf('w1 = %6.3f (rad/s) \n',w10)
fprintf('w2 = %6.3f (rad/s) \n',w2)
fprintf('w4 = %6.3f (rad/s) \n',w4)
fprintf('w5 = %6.3f (rad/s) \n',w5)
fprintf('\n')

fprintf('n1 = %6.3f (rpm) \n',30*w10/pi)
fprintf('n2 = %6.3f (rpm) \n',30*w2/pi)
fprintf('n4 = %6.3f (rpm) \n',30*w4/pi)
fprintf('n5 = %6.3f (rpm) \n',30*w5/pi)


% w1 = 70.162 (rad/s) 
% w2 =  1.257 (rad/s) 
% w4 = 37.350 (rad/s) 
% w5 =  5.563 (rad/s) 
% 
% n1 = 670.000 (rpm) 
% n2 = 12.000 (rpm) 
% n4 = 356.667 (rpm) 
% n5 = 53.125 (rpm) 

