clear all
clc
format long g

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

th1 = pi;

% input harmonic piston motion starting from ''upper dead point''
% NN = 6000*2*pi/60; %frequency in piston 6000 CPM (cycles per minute)
%     T = 2*pi/NN;
%     dt = T/100;
% for i = 1:200
%     t(i) = i*0.93*dt; %0.93 is a random number close enough to 1 so as to avoid cead points
%     r1(i) = r3 + r2*cos(NN*t(i));
%     r1t(i) = -r2*NN*sin(NN*t(i));
%     r1tt(i) = -r2*(NN^2)*cos(NN*t(i));
% end

%input piston motion for constant crank rotating speed omega2
%found theoretically from the 'crank-driven' kinematics
NN = 6000*2*pi/60; %desired frequency in crank 6000 RPM
    T = 2*pi/NN;
    dt = T/100;
for i = 1:200
    t(i) = i*0.93*dt; %0.93 is a random number close enough to 1 so as to avoid dead points
    r1(i) = r2*cos(NN*t(i)) + sqrt(r3^2 - (r2^2)*sin(NN*t(i))^2);
end

figure(1)
plot(t,1e3*r1)
xlabel('t [sec]')
ylabel('r_{1}, [mm]')
xlim([0 2*T])
grid on

% figure(2)
% plot(t,r1t)
% xlabel('t [sec]')
% ylabel('r1t, [m/s]')
% xlim([0 2*T])
% grid on
% 
% figure(3)
% plot(t,r1tt)
% xlabel('t [sec]')
% ylabel('r1tt, [m/s^2]')
% xlim([0 2*T])
% grid on

%POSITION ANALYSIS
for i = 1:length(r1)
b(i) = r1(i);
A(i) = (b(i)^2-r2^2+r3^2)/(2*b(i)*r3);
B(i) = (b(i)-r3*A(i))/r2;
C(i) = (r3/r2)*sqrt(1-A(i)^2);
end

% Crank Angle
for i = 1:length(r1)
    
cosath2(i) = B(i);

r1t(i) = -r2*NN*sin(NN*t(i)); %used only to indicate right anf left movement of the slider
if r1t(i)<0
    sinath2(i) = C(i);
else
    sinath2(i) = -C(i);
end
    
th2s(i) = asin(abs(sqrt(1-A(i)^2)*r3/r2));
    
if cosath2(i)>0 && sinath2(i)>=0
    th2(i) = th2s(i);
end

if cosath2(i)<0 && sinath2(i)>=0
    th2(i) = pi-th2s(i);
end

if cosath2(i)<0 && sinath2(i)<=0
    th2(i) = pi+th2s(i);
end

if cosath2(i)>0 && sinath2(i)<=0
    th2(i) = 2*pi-th2s(i);
end

end

figure(4)
plot(1e3*r1,180*th2/pi,'x-')
xlabel('r_{1} [mm]')
ylabel('\theta_{2}, [deg]')
xlim([1e3*(r3-r2) 1e3*(r3+r2)])
ylim([0 360])
grid on

figure(5)
plot(180*th2/pi,1e3*r1,'x-')
xlabel('\theta_{2}, [deg]')
ylabel('r_{1} [mm]')
xlim([0 360])
ylim([1e3*(r3-r2) 1e3*(r3+r2)])
grid on

figure(6)
plot(t,180*th2/pi,'x-')
xlabel('t [s]')
ylabel('\theta_{2}, [deg]')
xlim([0 2*T])
ylim([0 360])
grid on

% Rod Angle
for i = 1:length(r1)
    
cosath3(i) = (b(i)-r2*B(i))/r3;

if r1t(i)<0
    sinath3(i) = -C(i);
else
    sinath3(i) = C(i);
end

th3s(i) = asin(abs(-r2*C(i)/r3));
    
if cosath3(i)>0 && sinath3(i)>=0
    th3(i) = th3s(i);
end

if cosath3(i)<0 && sinath3(i)>=0
    th3(i) = pi-th3s(i);
end

if cosath3(i)<0 && sinath3(i)<=0
    th3(i) = pi+th3s(i);
end

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

end

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

figure(7)
plot(1e3*r1,180*th3/pi,'x-')
xlabel('r_{1} [mm]')
ylabel('\theta_{3}, [deg]')
xlim([1e3*(r3-r2) 1e3*(r3+r2)])
grid on

figure(8)
plot(t,180*th3/pi,'x-')
xlabel('t [s]')
ylabel('\theta_{3}, [deg]')
xlim([0 2*T])
grid on

% VELOCITY ANALYSIS
r1t = -r2*NN*sin(th2-th3)./cos(th3);%found theoretically from crank-driven problem

for i = 1:length(r1)
    omega2(i) = r1t(i)*cos(th3(i))/(r2*sin(th3(i)-th2(i)));
    omega3(i) = r1t(i)*cos(th2(i))/(r3*sin(th2(i)-th3(i)));
end

figure(9)
plot(1e3*r1,r1t,'x-')
xlabel('r_{1} [mm]')
ylabel('r1t, [m/s]')
xlim([1e3*(r3-r2) 1e3*(r3+r2)])
grid on

figure(10)
plot(1e3*r1,omega2,'x-')
xlabel('r_{1} [mm]')
ylabel('\omega_{2}, [rad/s]')
xlim([1e3*(r3-r2) 1e3*(r3+r2)])
grid on

figure(11)
plot(180*th2/pi,omega2,'x-')
xlabel('\theta_{2} [deg]')
ylabel('\omega_{2}, [rad/s]')
xlim([0 360])
grid on

figure(12)
plot(1e3*r1,omega3,'x-')
xlabel('r_{1} [mm]')
ylabel('\omega_{3}, [rad/s]')
xlim([1e3*(r3-r2) 1e3*(r3+r2)])
grid on

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

%ACCELERATION ANALYSIS
r1tt = (r1t.*omega3.*sin(th3)-r2*omega2.*(omega2-omega3).*cos(th2-th3))./cos(th3);
for i = 1:length(r1)
a2(i) = (-r2*omega2(i)*(omega3(i)-omega2(i))*cos(th3(i)-th2(i))+r1tt(i)*cos(th3(i))-r1t(i)*omega3(i)*sin(th3(i)))/(r2*sin(th3(i)-th2(i)));
a3(i) = (-r3*omega3(i)*(omega2(i)-omega3(i))*cos(th2(i)-th3(i))+r1tt(i)*cos(th2(i))-r1t(i)*omega2(i)*sin(th2(i)))/(r2*sin(th2(i)-th3(i)));
end

figure(14)
plot(1e3*r1,a2)
xlabel('r_{1} [mm]')
ylabel('\alpha_{2}, [rad/s^{2}]')
xlim([1e3*(r3-r2) 1e3*(r3+r2)])
grid on

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

figure(16)
plot(1e3*r1,a3)
xlabel('r_{1} [mm]')
ylabel('\alpha_{3}, [rad/s^{2}]')
xlim([1e3*(r3-r2) 1e3*(r3+r2)])
grid on

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

figure(18)
plot(t,a2,t,a3)
xlabel('t [s]')
ylabel('\alpha_{2},\alpha_{3}, [rad/s^{2}]')
xlim([0 2*T])
grid on


%FORCE ANALYSIS
r2G = r2/2;
r3G = r3/2;
m2 = 0.042;
m3 = 0.080;
m4 = 0.060;
I02 = (1/12)*m2*(r2^2);
I03 = (1/12)*m3*(r3^2);

%ACCELERATION IN G2, G3, G4
%Carefull!! now a2 is in the equations!! 
AG2_x = -r2G*(cos(th2).*(omega2.^2)+sin(th2).*a2);
AG2_y = -r2G*(sin(th2).*(omega2.^2)-cos(th2).*a2);
AG3_x = -r2*((omega2.^2).*cos(th2)+a2.*sin(th2))-r3G*(cos(th3).*(omega3.^2)+sin(th3).*a3);
AG3_y = -r2*((omega2.^2).*sin(th2)-a2.*cos(th2))-r3G*(sin(th3).*(omega3.^2)-cos(th3).*a3);
AG4_x = r1tt;
AG4_y = zeros(1,length(r1));

AG2_ksi = -r2G*(omega2.^2);
AG2_hta = r2G*a2;
AG3_ksi = -r3G*omega3.^2-r2*((omega2.^2).*cos(th2-th3)+a2.*sin(th2-th3));
AG3_hta = r3G*a3+r2*a2.*cos(th2-th3)-r2*(omega2.^2).*sin(th2-th3);
AG4_ksi = r1tt;
AG4_hta = zeros(1,length(th2));

% INERTIA FORCES/MOMENTS
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*a2;
M3_I = -I03*a3;
M4_I = zeros(1,length(th2));

%FORCE ANALYSIS
% P_4 = 200*ones(1,length(th2)); %Input Force example

AM = 18000; %define here your AM!!!
PRE = load('pressure.txt');
%you may see the normalized pressure here
figure(181)
plot(PRE(:,1),PRE(:,2))
xlabel('\theta_{2} [deg]')
ylabel('normalized Pressure P')
xlim([0 720])
grid on

for i = 1:length(r1)
    
    %Example of Input Force as a function of position/velocity
%     if r1t(i)<0
%         P_4(i) = -2000;
%     else
%         P_4(i) = 2000;
%     end
   
%     Example of Input Force in piston of a 4-stroke cylinder compustion engine (motorcycle)
    P_4(i) = AM*(PRE(i,2)-0.1); %0.1 is the ambient pressure

F34ksi(i) = P4_Iksi(i) + P_4(i);
F23hta(i) = (-M3_I(i) - r3G*P3_Ihta(i))/r3;
F43hta(i) = P3_Ihta(i) + F23hta(i);
F43ksi(i) = (F34ksi(i) + F43hta(i)*sin(th3(i)))/cos(th3(i));

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

BB(1) = -P2_Iksi(i);
BB(2) = -P3_Iksi(i)-F43ksi(i);
BB(3) = 0;
BB(4) = M2_I(i) - r2G*P2_Ihta(i);
BB(5) = F23hta(i)*sin(th3(i));
BB(6) = - F23hta(i)*cos(th3(i));
BB(7) = P2_Ihta(i);
BB(8) = F43ksi(i)*sin(th3(i))+F43hta(i)*cos(th3(i));

sol = linsolve(AA,BB');

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

end

%transformation of forces from ksi-eta to x-y
F12x = cos(th2).*F12ksi - sin(th2).*F12hta;
F12y = sin(th2).*F12ksi + cos(th2).*F12hta;

F14x = 0;
F14y = F14;

F23x = cos(th3).*F23ksi - sin(th3).*F23hta;
F23y = sin(th3).*F23ksi + cos(th3).*F23hta;

F32x = cos(th2).*F32ksi - sin(th2).*F32hta;
F32y = sin(th2).*F32ksi + cos(th2).*F32hta;

F34x = F34ksi;
F34y = F34hta;

F43x = cos(th3).*F43ksi - sin(th3).*F43hta;
F43y = sin(th3).*F43ksi + cos(th3).*F43hta;

%magnitude of forces
F12 = sqrt(F12x.^2+F12y.^2); %magnitude of F12 force
F14 = sqrt(F14x.^2+F14y.^2); %magnitude of F14 force
F23 = sqrt(F23x.^2+F23y.^2); %magnitude of F23 force
F32 = sqrt(F32x.^2+F32y.^2); %magnitude of F32 force
F34 = sqrt(F34x.^2+F34y.^2); %magnitude of F32 force
F43 = sqrt(F43x.^2+F43y.^2); %magnitude of F43 force

%Force F12 (crank Bearing Force)
figure(19)
plot(t,F12ksi,t,F12hta,t,F12x,t,F12y,t,F12)
xlabel('t [s]')
legend('F_{12}^{\xi}','F_{12}^{\eta}','F_{12}^{x}','F_{12}^{y}','|F_{12}|')
ylabel('F_{12} [N]')
xlim([0 2*T])
grid on

%Force F23 (pin join force)
figure(20)
plot(t,F23ksi,t,F23hta,t,F23x,t,F23y,t,F23)
xlabel('t [s]')
legend('F_{23}^{\xi}','F_{23}^{\eta}','F_{23}^{x}','F_{23}^{y}','|F_{23}|')
ylabel('F_{23} [N]')
xlim([0 2*T])
grid on

%Force F32 (pin join force)
figure(21)
plot(t,F32ksi,t,F32hta,t,F32x,t,F32y,t,F32)
xlabel('t [s]')
legend('F_{32}^{\xi}','F_{32}^{\eta}','F_{32}^{x}','F_{32}^{y}','|F_{32}|')
ylabel('F_{32} [N]')
xlim([0 2*T])
grid on

%Force F34
figure(22)
plot(t,F34ksi,'o-',t,F34hta,'o-',t,F34x,'x',t,F34y,'x',t,F34)
xlabel('t [s]')
legend('F_{34}^{\xi}','F_{34}^{\eta}','F_{34}^{x}','F_{34}^{y}','|F_{34}|')
ylabel('F_{34} [N]')
xlim([0 2*T])
grid on

%Force F43
figure(23)
plot(t,F43ksi,'o-',t,F43hta,'o-',t,F43x,'x',t,F43y,'x',t,F43)
xlabel('t [s]')
legend('F_{43}^{\xi}','F_{43}^{\eta}','F_{43}^{x}','F_{43}^{y}','|F_{43}|')
ylabel('F_{43} [N]')
xlim([0 2*T])
grid on

%Force F14 (frame force in piston) 
figure(24)
plot(t,F14y)
xlabel('t [s]')
ylabel('F_{14} [N]')
xlim([0 2*T])
grid on

%Torque M2 in the crank 
% -M2 is the torque load in crankshaft
% +M2 is the resisting torque to provide equilibrium of forces in the
% mechanism
figure(25)
plot(t,-M2) 
xlabel('t [s]')
ylabel('M_{2} [Nm]')
xlim([0 2*T])
grid on

% EVALUATION OF FLYWHEEL FOR Cs = 0.17
dth = omega2.*dt; 
W_cycle = sum(-M2.*dth); %Work in a force period 2T
M_mean = W_cycle/(4*pi); %Mean Torque 

figure(26)
plot(t,-M2,t,M_mean*ones(1,length(M2))) 
legend('M_{2}','M_{mean}')
xlabel('t [s]')
ylabel('M_{2},M_{mean} [Nm]')
xlim([0 2*T])
grid on

%Calculation of Delta Energy
DE = 0;
for i=1:length(M2)
    if -M2(i)>M_mean
        DE = DE + (-M2(i)-M_mean)*dth(i);
    end
end

%calculation of flywheel
Cs = 0.17;
I_f = DE/((NN^2)*Cs);
m_f = 2; %flywheel mass
R_f = sqrt(I_f/m_f);