% simple planetary gear train
% velocity analysis

clear; clc; close all

N1 = 30; % no. of teeth of sun gear 1
N2 = 15; % no. of teeth of planet gear 2
m = 22*10^-3; % (m) module of the gears

n1 = 50; % (rpm) angular velocity of 1
omega1 = n1*pi/30;   % (rad/s)
omega1_ = [0 0 omega1];

% pitch radii
r1 = m*N1/2; % (m) pitch radius of sun gear 1
r2 = m*N2/2; % (m) pitch radius of planet gear 1
r4 = r1 + 2*r2;  % (m)  pitch radius of ring gear 4
N4 = 2*r4/m; % no. of teeth of ring gear 4

fprintf('r1 = %6.3f (m) \n',r1)
fprintf('r2 = %6.3f (m) \n',r2)
fprintf('r4 = %6.3f (m) \n',r4)
fprintf('N4 = %g (teeth) \n',N4)

% r1 =  0.330 (m) 
% r2 =  0.165 (m) 
% r4 =  0.660 (m) 
% N4 = 60 (teeth) 

% origin of the reference frame 
% position vector of center of gear 1: O1=O
rO1_ = [0 0 0];
% position vector of center of gear 4: O4
rO4_ = rO1_;
% position vector of center of planer gear 2: O2
rO2_ = [0 -r1-r2 0];
% position vector of pitch point A (gears 1 and 2)
rA_ = [0 -r1 0];
% position vector of pitch point B (gears 2 and 4)
rB_ = [0 -r4 0];

fprintf('rO1_=rO4_=rO_= [%g %g %g] (m) \n',rO1_)
fprintf('rO2_ = [%g %6.3f %g] (m) \n',rO2_)
fprintf('rA_ = [%g %6.3f %g] (m) \n',rA_)
fprintf('rB_ = [%g %6.3f %g] (m) \n',rB_)
fprintf('\n')
% rO1_=rO4_=rO_= [0 0 0] (m) 
% rO2_ = [0 -0.495 0] (m) 
% rA_ = [0 -0.330 0] (m) 
% rB_ = [0 -0.660 0] (m) 

figure(1)
af = 1;
axis([-af af -af af])

axis equal
axis square
grid on
hold on

xlabel('x(m)'), ylabel('y(m)'), zlabel('z(m)')
t = linspace(0,2*pi);
% plot gear 1 of radius r1 at O1
plot(rO1_(1)+r1*cos(t),rO1_(2)+r1*sin(t),...
 'color','b','LineWidth',3) 
% plot gear 2 of radius r2 at O2
plot(rO2_(1)+r2*cos(t),rO2_(2)+r2*sin(t),...
 'color','r','LineWidth',3) 
plot(rO4_(1)+r4*cos(t),rO4_(2)+r4*sin(t),...
 'color','k','LineWidth',3) 
text(rO1_(1),rO1_(2)+0.04,' O','fontsize',12)
text(rO2_(1),rO2_(2),' O_2','fontsize',12)

text(rA_(1),rA_(2)+0.04,' A','fontsize',12)
text(rB_(1),rB_(2)+0.04,' B','fontsize',12)

plot(rO1_(1),rO1_(2),'o','color','k')
plot(rO2_(1),rO2_(2),'o','color','k')

plot(rA_(1),rA_(2),'o','color','r')
plot(rB_(1),rB_(2),'o','color','r')

line([rO1_(1),rO2_(1)],[rO1_(2),rO2_(2)],...
    'LineStyle','-','LineWidth',4)

line([-af,af],[0,0],...
    'LineStyle','-.','LineWidth',1.5)
line([0,0],[-af,af],...
    'LineStyle','-.','LineWidth',1.5)


% classical method

vO_ = [0 0 0]; % velocity of O on sun gear 1
% velocity of A=A1 on sun gear 1
vA1_ = vO_ + cross(omega1_, rA_); 
% vA1_ = [ 1.728 0 0] (m/s) 

fprintf('vA1_ = [%6.3f %g %g] (m/s) \n',vA1_)
fprintf('\n')

% omega2 unknown
syms omega2 
omega2_ = [0 0 omega2]; % angular velocity of planet 2
% vB2_ is the velocity of B2 on planet gear 2 
% vB4_ is the velocity of B4 on fixed ring gear 4
% vB2_ = vB4_ = [0 0 0]
vB2_ = [0 0 0];
% velocity of A2 on planet gear 2 
vA2_ =  vB2_ + cross(omega2_, rA_-rB_);
% vA1_ = vA2_ 
eqvA_ = vA1_-vA2_;
eqvA  = eqvA_(1); % (1)
% Eq.(1) =>
omega2  = eval(solve(eqvA));
omega2_ = [0 0 omega2];
% omega2=-5.236(rad/s)=-50.000(rpm)

fprintf('\n')
fprintf('omega2=%6.3f(rad/s)=%6.3f(rpm)\n',...
    omega2,omega2*30/pi)

% omega3 unknown
syms omega3
omega3_ = [0 0 omega3]; % angular velocity of planet arm 3
% vO22_ is the velocity of O2=O22 on planet gear 2 
% vO23_ is the velocity of O2=O23 on planet arm 3
vO22_ = vB2_ + cross(omega2_,rO2_-rB_);
vO23_ = vO_ + cross(omega3_,rO2_);
% vO22_ = vO23_
eqvO2_ = vO22_-vO23_; 
eqvO2 = eqvO2_(1); % (2)
% Eq.(2) =>
omega3  = eval(solve(eqvO2));
omega3_ = [0 0 omega3];
% omega3= 1.745(rad/s)=16.667(rpm)

fprintf('omega3=%6.3f(rad/s)=%6.3f(rpm)\n',...
    omega3,omega3*30/pi)


% contour method

% contour 0-1-2-4=0 
omega10_ = [0 0 omega1 ];
syms omega21 omega02
% omega21 is the relative angular velocity of 2 with respect to 1 at A
% omega02 is the relative angular velocity of 0=4 with respect to 2 at B
omega21_ = [0 0 omega21];
omega02_ = [0 0 omega02];
% omega10_ + omega21_ + omega02_ = 0_
% OA_ x omega21_ + OB_ x omega02_ = 0_
eqI1_ = omega10_ + omega21_ + omega02_;
eqI1 = eqI1_(3); % (3) 
eqI2_ = cross(rA_,omega21_)+cross(rB_,omega02_);
eqI2 = eqI2_(1); % (4)

% Eqs.(3)(4)=>
solI = solve(eqI1,eqI2);
omega21 = eval(solI.omega21);
omega02 = eval(solI.omega02);
omega20 = - omega02;

% omega21 = -10.472 (rad/s)
% omega20 = -5.236 (rad/s)


fprintf('\n')
fprintf('omega21 = %6.3f (rad/s)\n',omega21)
fprintf('omega20 = %6.3f (rad/s)\n',omega20)

% contour 0-1-2-3-0
omega21_ = [0 0 omega21];
syms omega32 omega03
% omega32 is the relative angular velocity of 3 with respect to 2 at O2
% omega03 is the relative angular velocity of 0 with respect to 3 at O
omega32_ = [0 0 omega32];
omega03_ = [0 0 omega03];

% omega10_ + omega21_ + omega32_ + omega03_ = 0_
% OA_ x omega21_ + OO2_ x omega32_ = 0_

eqII1_ = omega10_ + omega21_ + omega32_ + omega03_;
eqII1 = eqII1_(3); % (5)
eqII2_ = cross(rA_, omega21_)+cross(rO2_, omega32_);
eqII2 = eqII2_(1); % (6)
% Eqs.(5)(6)=>
solII = solve(eqII1,eqII2);
omega32 = eval(solII.omega32);
omega03 = eval(solII.omega03);
omega30 = - omega03;

% omega32 =  6.981 (rad/s) 
% omega30 =  1.745 (rad/s) 


fprintf('\n')
fprintf('omega32 = %6.3f (rad/s) \n',omega32)
fprintf('omega30 = %6.3f (rad/s) \n',omega30)

fprintf('\n')
fprintf('omega20 = %6.3f (rpm) \n',omega20*30/pi)
fprintf('omega30 = %6.3f (rpm) \n',omega30*30/pi)

% eof

% r1 =  0.330 (m) 
% r2 =  0.165 (m) 
% r4 =  0.660 (m) 
% 
% omega2=-5.236(rad/s)=-50.000(rpm)
% omega3= 1.745(rad/s)=16.667(rpm)
% 
% omega21 = -10.472 (rad/s)
% omega20 = -5.236 (rad/s)
% 
% omega32 =  6.981 (rad/s) 
% omega30 =  1.745 (rad/s) 
% 
% omega20 = -50.000 (rpm) 
% omega30 = 16.667 (rpm) 
