clear all
clc
format long g

r2 = 0.042; %crank length
r3 = 0.135; %rod length

th1 = pi;
th2 = 0.01:(2*pi/100):2.01*pi; %angle taken from IDC
omega2 = 6000*2*pi/60;

n = r3/r2; %obliquity

%POSITION ANALYSIS
% Piston Motion
alpha = th2 + pi;
r1 = r2*cos(alpha-th1) + sqrt(r3^2 - (r2^2)*sin(alpha-th1).^2);

figure(1)
plot(th2*180/pi,r1)
xlabel('\theta_{2} [deg]')
ylabel('Displacement, r1 [m]')
xlim([0 360])
grid on

%rod angle
b = r2;
th3s = th1 + asin(abs(b*sin(alpha-th1)/r3));
sinth3th1 = b*sin(alpha-th1)/r3;
costh3th1 = (b*cos(alpha-th1)-r1)/r3;

% th3 = (atan2(sinth3th1,costh3th1) + pi)'; you may use this fucntion to
% calculate th3, instead of the following ''for loop''

for i = 1:length(th2)
    
if costh3th1(i)>0 && sinth3th1(i)>=0
    th3(i) = th3s(i);
end

if costh3th1(i)<0 && sinth3th1(i)>=0
    th3(i) = pi-th3s(i);
end

if costh3th1(i)<0 && sinth3th1(i)<=0
    th3(i) = pi+th3s(i);
end

if costh3th1(i)>0 && sinth3th1(i)<=0
    th3(i) = 2*pi-th3s(i);
end

end

for i = 1:length(th2)
    if th3(i)>2*pi
        th3(i) = th3(i) -  2*pi;
    end
end

figure(2)
plot(th2*180/pi,th3*180/pi)
xlabel('\theta_{2} [deg]')
ylabel('\theta_{3} [deg]')
xlim([0 360])
grid on

% VELOCITY ANALYSIS
for i = 1:length(th2)
omega3(i) = -r2*cos(th2(i) + pi)*omega2/(r3*cos(th3(i)-pi));
r1t(i) = -r2*omega2*sin(th2(i)-th3(i))/cos(th3(i));
end

figure(3)
plot(th2*180/pi,omega3)
xlabel('\theta_{2} [deg]')
ylabel('\Omega_{3} [rad/s]')
xlim([0 360])
grid on

figure(4)
plot(th2*180/pi,r1t)
xlabel('\theta_{2} [deg]')
ylabel('dr_{1}/dt [m/s]')
xlim([0 360])
grid on

%ACCELERATION ANALYSIS
for i = 1:length(th2)
alpha2(i) = 0;
alpha3(i) = (r2*(omega2^2)*sin(th2(i)) + r3*(omega3(i)^2)*sin(th3(i)))/(r3*cos(th3(i)));
r1tt(i) = (1/cos(th3(i)))*(r1t(i)*omega3(i)*sin(th3(i))-r2*omega2*(omega2-omega3(i))*cos(th2(i)-th3(i)));
end

figure(5)
plot(th2*180/pi,alpha3)
xlabel('\theta_{2} [deg]')
ylabel('\alpha_{3} [rad/s^{2}]')
xlim([0 360])
grid on

figure(6)
plot(th2*180/pi,r1tt)
xlabel('\theta_{2} [deg]')
ylabel('d^{2}r_{1}/dt^{2} [m/s^{2}]')
xlim([0 360])
grid on

% INERTIA FORCES
r2G = r2/2;
r3G = r3/2;
m2 = 0.042;
m3 = 0.135;
m4 = 0.100;
I02 = (1/12)*m2*(r2^2);
I03 = (1/12)*m3*(r3^2);

%ACCELERATION IN G2, G3, G4, A
%first tranform omega2 from a value to a vector of constant values 'omega2'
OMEGA2 = omega2*ones(1,length(th2));
%now omega2 is a vector of length(th2) elements

AG2_x = -r2G*cos(th2).*(OMEGA2.^2);
AG2_y = -r2G*sin(th2).*(OMEGA2.^2);
AG3_x = -r2*(OMEGA2.^2).*cos(th2)-r3G*(cos(th3).*(omega3.^2)+sin(th3).*alpha3);
AG3_y = -r2*(OMEGA2.^2).*sin(th2)-r3G*(sin(th3).*(omega3.^2)-cos(th3).*alpha3);
AG4_x = r1tt;
AG4_y = zeros(1,length(th2));
AA_x = -r2*cos(th2).*(OMEGA2.^2);
AA_y = -r2*sin(th2).*(OMEGA2.^2);

AG2_ksi = -r2G*(OMEGA2.^2);
AG2_hta = zeros(1,length(th2));
AG3_ksi = -r3G*omega3.^2-r2*(OMEGA2.^2).*cos(th2-th3);
AG3_hta = r3G*alpha3-r2*(OMEGA2.^2).*sin(th2-th3);
AG4_ksi = r1tt;
AG4_hta = zeros(1,length(th2));
AA_ksi = -r2*OMEGA2.^2;
AA_hta = zeros(1,length(th2));

P2_Iksi = -m2*AG2_ksi;
P2_Ihta = -m2*AG2_hta;
P3_Iksi = -m3*AG3_ksi;
P3_Ihta = -m3*AG3_hta;
P4_Iksi = -m4*AG4_ksi;
P4_Ihta = -m4*AG4_hta;
M2_I = -I02*alpha2;
M3_I = -I03*alpha3;
M4_I = zeros(1,length(th2));

figure(7)
plot(th2*180/pi,AG2_x,th2*180/pi,AG2_y,th2*180/pi,AG2_ksi,th2*180/pi,AG2_hta)
xlabel('\theta_{2} [deg]')
ylabel('A_{G2} [m/s^{2}]')
legend('A_{G2,x}','A_{G2,y}','A_{G2,\xi}','A_{G2,\eta}')
xlim([0 360])
grid on

figure(8)
plot(th2*180/pi,AG3_x,th2*180/pi,AG3_y,th2*180/pi,AG3_ksi,th2*180/pi,AG3_hta)
xlabel('\theta_{2} [deg]')
ylabel('A_{G3} [m/s^{2}]')
legend('A_{G3,x}','A_{G3,y}','A_{G3,\xi}','A_{G3,\eta}')
xlim([0 360])
grid on

figure(9)
plot(th2*180/pi,AG4_x,th2*180/pi,AG4_y,th2*180/pi,AG4_ksi,'o',th2*180/pi,AG4_hta,'o')
xlabel('\theta_{2} [deg]')
ylabel('A_{G4} [m/s^{2}]')
legend('A_{G4,x}','A_{G4,y}','A_{G4,\xi}','A_{G4,\eta}')
xlim([0 360])
grid on

figure(10)
plot(th2*180/pi,AA_x,th2*180/pi,AA_y,th2*180/pi,AA_ksi,th2*180/pi,AA_hta)
xlabel('\theta_{2} [deg]')
ylabel('A_{A} [m/s^{2}]')
legend('A_{A,x}','A_{A,y}','A_{A,\xi}','A_{A,\eta}')
xlim([0 360])
grid on

figure(11)
plot(th2*180/pi,P2_Iksi,th2*180/pi,P2_Ihta,th2*180/pi,P3_Iksi,th2*180/pi,P3_Ihta,th2*180/pi,P4_Iksi,th2*180/pi,P4_Ihta,'o')
xlabel('\theta_{2} [deg]')
ylabel('P^{I} [N]')
legend('P_{2}^{I\xi}','P_{2}^{I\eta}','P_{3}^{I\xi}','P_{3}^{I\eta}','P_{4}^{I\xi}','P_{4}^{I\eta}')
xlim([0 360])
grid on

figure(12)
plot(th2*180/pi,M2_I,th2*180/pi,M3_I,th2*180/pi,M4_I,'o')
xlabel('\theta_{2} [deg]')
ylabel('M^{I} [Nm]')
legend('M_{2}^{I}','M_{3}^{I}','M_{4}^{I}')
xlim([0 360])
grid on

% NUMERICAL SOLUTION
M_2 = 50; %Input Torque 50 Nm
for i = 1:length(th2)

F12hta(i) = (-M_2 + M2_I(i) - r2G*P2_Ihta(i))/r2;
F32hta(i) = -F12hta(i) + P2_Ihta(i);
F23hta(i) = (-M3_I(i) - r3G*P3_Ihta(i))/r3;
F43hta(i) = P3_Ihta(i) + F23hta(i);

A = zeros(8,8);
A(1,1) = 1; A(1,2) = -1;
A(2,3) = -1; A(2,4) = -1;
A(3,3) = 1; A(3,7) = 1;
A(4,5) = 1; A(4,8) = -1;
A(5,2) = -cos(th2(i)); A(5,3) = -cos(th3(i));
A(6,2) = -sin(th2(i)); A(6,3) = -sin(th3(i));
A(7,4) = -cos(th3(i)); A(7,5) = 1;
A(8,4) = -sin(th3(i)); A(8,6) = 1;

B(1) = -P2_Iksi(i);
B(2) = P3_Iksi(i);
B(3) = 0;
B(4) = P4_Iksi(i);
B(5) = -F32hta(i)*sin(th2(i)) + F23hta(i)*sin(th3(i));
B(6) = F32hta(i)*cos(th2(i)) - F23hta(i)*cos(th3(i));
B(7) = -F43hta(i)*sin(th3(i));
B(8) = F43hta(i)*cos(th3(i));

sol = linsolve(A,B');

F12ksi(i) = sol(1);
F32ksi(i) = sol(2);
F23ksi(i) = sol(3);
F43ksi(i) = sol(4);
F34ksi(i) = sol(5);
F34hta(i) = sol(6);
F14(i) = sol(7);
 P4(i) = sol(8);
 
sprintf('%0.5g',i) 

end

figure(13)
%Piston Force
plot(th2*180/pi,P4,'kx-')
xlabel('\theta_{2} [rad]')
ylabel('P_{4} [N]')
xlim([0 360])
grid on

%Bearing Force
figure(14)
F12 = sqrt(F12ksi.^2+F12hta.^2); %magnitude of F12 vector
plot(th2*180/pi,F12,'kx-')
xlabel('\theta_{2} [rad]')
ylabel('F_{12} [N]')
xlim([0 360])
grid on

%Force in the frame from the piston
figure(15)
plot(th2*180/pi,F14,'kx-')
xlabel('\theta_{2} [rad]')
ylabel('F_{14} [N]')
xlim([0 360])
grid on

%Pin Force
figure(16)
F32 = sqrt(F32ksi.^2+F32hta.^2);  %magnitude of F32 vector
plot(th2*180/pi,F32,'kx-')
xlabel('\theta_{2} [rad]')
ylabel('F_{32} [N]')
xlim([0 360])
grid on

%Speed in joint (pin speed)
figure(17)
plot(180*th2/pi,omega3-omega2) 
xlabel('\theta_{2} [rad]')
ylabel('speed in joint (\omega_{3}-\omega_{2}) [rad/s]')
xlim([0 360])
grid on

%Rod Force axial
figure(18)
plot(th2*180/pi,F43ksi,'kx-')
xlabel('\theta_{2} [rad]')
ylabel('F_{43}^{\xi} [N]')
xlim([0 360])
grid on