clear all
close all

%% inputs %%
a=1.5;
b=7+3/8;
c=4+3/8;
theta1=0;
g1=4+3/16;
g2y=1.5;

speed=1;
final=500;
%%for 1 cycle uncomment below%%
speed=pi/10;
final=200;

%% angular position
for i=1:final
    omega2(i)=speed;
    theta2(i)=speed*i/10;
    theta3(i)=atan2(-g1*sin(theta1)+a*sin(theta2(i)),-g1*cos(theta1)+a*cos(theta2(i)));
    b1(i)=(-g1*cos(theta1)+a*cos(theta2(i)))/cos(theta3(i));
    b2(i)=b-b1(i);
    theta4(i)=asin((-b2(i)*sin(theta3(i))+g2y)/c);
    theta4(i)=pi-theta4(i);
    g2x(i)=-c*cos(theta4(i))-b2(i)*cos(theta3(i));
end

%% angular velocity %%
for i=1:final
    omega3(i)=(a*omega2(i)*cos(theta2(i))+tan(theta3(i))*a*omega2(i)*sin(theta2(i)))/...
        (b1(i)*cos(theta3(i))+b1(i)*sin(theta3(i))*tan(theta3(i)));
    b1dot(i)=(b1(i)*omega3(i)*sin(theta3(i))-a*omega2(i)*sin(theta2(i)))/cos(theta3(i));
    
    b2dot(i)=-b1dot(i);
    
    omega4(i)=(-b2(i)*omega3(i)*cos(theta3(i))-b2dot(i)*sin(theta3(i)))/(c*cos(theta4(i)));
    g2xdot(i)=c*omega4(i)*cos(theta4(i))+b2(i)*omega3(i)*sin(theta3(i))+b2dot(i)*cos(theta3(i));
end

%% angular acceleration %%
for i=1:final
    alpha2(i)=0;
    alpha3(i)=(tan(theta3(i))*(-omega2(i)^2*a*cos(theta2(i))+omega3(i)^2*b1(i)*cos(theta3(i))...
        +2*omega3(i)*sin(theta3(i)))+omega2(i)^2*a*sin(theta2(i))-omega3(i)^2*b1(i)*sin(theta3(i))...
        +2*omega3(i)*b1dot(i)*cos(theta3(i)))/(b1(i)*cos(theta3(i))+tan(theta3(i))*b1(i)*sin(theta3(i)));   
    b1dot2(i)=(-omega2(i)^2*a*cos(theta2(i))+omega3(i)^2*b1(i)*cos(theta3(i))+alpha3(i)...
        *b1(i)*sin(theta3(i))+2*omega3(i)*b1dot(i)*sin(theta3(i)))/cos(theta3(i));
    b2dot2(i)=-b1dot(i);
    
    alpha4(i)=(omega3(i)^2*b2(i)*sin(theta3(i))-b2dot2(i)*sin(theta3(i))+c*omega4(i)^2*sin(theta4(i))...
        -b2(i)*alpha3(i)*cos(theta3(i))-2*b2dot(i)*omega3(i)*cos(theta3(i)))/(c*cos(theta4(i)));
    gx2dot2(i)=c*alpha4(i)*sin(theta4(i))+omega3(i)^2*b2(i)*cos(theta3(i))-b2dot2(i)*cos(theta3(i))...
        +c*omega4(i)^2*cos(theta4(i))+b2(i)*alpha3(i)*sin(theta3(i))+2*b2dot(i)*omega3(i)*cos(theta3(i));
    
%     s_coriolis(i)=2*b2dot(i)*omega

     g_coriolis(i)=2*omega3(i)*b1dot(i);
end

%% point kinematics %%
for i=1:final
  %position
  p1x(i)=0;
  p1y(i)=0;
  p2x(i)=a*cos(theta2(i));
  p2y(i)=a*sin(theta2(i));
  p3x(i)=g1;
  p3y(i)=0;
  p4x(i)=p3x(i)-b2(i)*cos(theta3(i));
  p4y(i)=p3y(i)-b2(i)*sin(theta3(i));
  p5x(i)=p4x(i)-c*cos(theta4(i));
  p5y(i)=p4y(i)-c*sin(theta4(i));
  
  %velocity
  v1x(i)=0;
  v1y(i)=0;
  v2x(i)=-omega2(i)*a*sin(theta2(i));
  v2y(i)=omega2(i)*a*cos(theta2(i));
  v3x(i)=0;
  v3y(i)=0;
  v4x(i)=omega3(i)*b2(i)*sin(theta3(i))-b2dot(i)*cos(theta3(i));
  v4y(i)=-omega3(i)*b2(i)*cos(theta3(i))-b2dot(i)*sin(theta3(i));
  v5x(i)=omega4(i)*c*sin(theta4(i))+v4x(i);
  v5y(i)=0;
  
  %acceleration
  a1x(i)=0;
  a1y(i)=0;
  a2x(i)=-alpha2(i)*a*sin(theta2(i))-omega2(i)^2*a*cos(theta2(i));
  a2y(i)=alpha2(i)*a*cos(theta2(i))-omega2(i)^2*a*sin(theta2(i));
  a3x(i)=0;
  a3y(i)=0;
  a4x(i)=alpha3(i)*b2(i)*sin(theta3(i))+omega3(i)^2*b2(i)*cos(theta3(i))...
      +2*omega3(i)*b2dot(i)*sin(theta3(i))-b2dot2(i)*cos(theta3(i));
  a4y(i)=-alpha3(i)*b2(i)*cos(theta3(i))+omega3(i)^2*b2(i)*sin(theta3(i))...
      -2*omega3(i)*b2dot(i)*cos(theta3(i))-b2dot2(i)*sin(theta3(i));
  a5x(i)=a4x(i)+alpha4(i)*c*sin(theta4(i))+omega4(i)^2*c*cos(theta4(i));
  a5y(i)=0;
end

%% animation %%
axis(gca,'equal')
axis([-3 15 -3 10]);
P1=[0 0];
title('Slotted 5 bar with slider')
for i=1:final
    theta2a=speed*i/10;
    
    theta3a=theta3(i);
    theta4a=theta4(i);
    b1a=b1(i);
    b2a=b2(i);
    g2xa=g2x(i);

    P2=[a*cos(theta2a) a*sin(theta2a)];
    P3=[P2(1)-b1a*cos(theta3a) P2(2)-b1a*sin(theta3a)];
    P4=[P3(1)-b2a*cos(theta3a) P3(2)-b2a*sin(theta3a)];
    P5=[P4(1)-c*cos(theta4a) P4(2)-c*sin(theta4a)];
   
    P6=[5 P5(2)];
    P7=[12 P5(2)];
    
    if i>1
        if g2x(i)>g2x(i-1)
            direction=1;
        else
            direction=-1;
        end
    else
        direction=1;
    end
    P8=[P5(1)+direction*2 P5(2)];
    
    if i>1
        if b1(i)>b1(i-1)
            direction2=1;
        else
            direction2=-1;
        end
    else
        direction2=1;
    end
    P9=[P3(1)+abs(g_coriolis(i))*cos(theta3a+direction2*pi/2), P3(2)+abs(g_coriolis(i))*sin(theta3a+direction2*pi/2)];
    
    crank=line([P1(1),P2(1)],[P1(2),P2(2)]);
    slot1=line([P2(1) P3(1)],[P2(2),P3(2)]);
    slot2=line([P3(1) P4(1)],[P3(2),P4(2)]);
    coupler=line([P4(1) P5(1)],[P4(2),P5(2)]);
%     vertical=line([P6(1) P7(1)],[P6(2) P7(2)]);
%     slider_coriolis=line([P5(1) P8(1)],[P5(2) P8(2)]);
    ground_coriolis=line([P3(1) P9(1)],[P3(2) P9(2)]);
    
    P1_circ=viscircles(P1,0.1);
    P2_circ=viscircles(P2,0.1);
    P3_circ=viscircles(P3,0.1);
    P4_circ=viscircles(P4,0.1);
    P5_circ=viscircles(P5,0.1);
  
   
    pause(.001);
    
    delete(crank)
    delete(slot1)
    delete(slot2)
    delete(coupler)
    delete(P1_circ)
    delete(P2_circ)
    delete(P3_circ)
    delete(P4_circ)
    delete(P5_circ)
    delete(ground_coriolis)
end

%% Plots for points %%
figure (2)
plot(theta2,v1x)
hold on
plot(theta2,v2x)
hold on
plot(theta2,v3x)
hold on
plot(theta2,v4x)
hold on
plot(theta2,v5x)
grid on
legend('Point 1', 'Point 2', 'Point 3', 'Point 4', 'Point 5');
title('Velocity in x direction')
ylabel('Velocity (in/s)')
xlabel('Theta2 (rad)')

figure (3)
plot(theta2,v1y)
hold on
plot(theta2,v2y)
hold on
plot(theta2,v3y)
hold on
plot(theta2,v4y)
hold on
grid on
plot(theta2,v5y)
legend('Point 1', 'Point 2', 'Point 3', 'Point 4', 'Point 5');
title('Velocity in y direction')
xlabel('Theta2 (rad)')
ylabel('Velocity (in/s)')

figure (4)
plot(theta2,a1x)
hold on
plot(theta2,a2x)
hold on
plot(theta2,a3x)
hold on
plot(theta2,a4x)
hold on
grid on
plot(theta2,a5x)
legend('Point 1', 'Point 2', 'Point 3', 'Point 4', 'Point 5');
title('Acceleration in x direction')
xlabel('Theta2 (rad)')
ylabel('Acceleration (in/s^2)')

figure (5)
plot(theta2,a1y)
hold on
plot(theta2,a2y)
hold on
plot(theta2,a3y)
hold on
plot(theta2,a4y)
hold on
grid on
plot(theta2,a5y)
legend('Point 1', 'Point 2', 'Point 3', 'Point 4', 'Point 5');
title('Acceleration in y direction')
xlabel('Theta2 (rad)')
ylabel('Acceleration (in/s^2)')

figure (6)
plot(theta2,p1x)
hold on
plot(theta2,p2x)
hold on
plot(theta2,p3x)
hold on
plot(theta2,p4x)
hold on
grid on
plot(theta2,p5x)
legend('Point 1', 'Point 2', 'Point 3', 'Point 4', 'Point 5');
title('Position along x axis')
xlabel('Theta2 (rad)')
ylabel('Position (in)')

figure(7)
plot(theta2,p1y)
hold on
plot(theta2,p2y)
hold on
plot(theta2,p3y)
hold on
plot(theta2,p4y)
hold on
grid on
plot(theta2,p5y)
legend('Point 1', 'Point 2', 'Point 3', 'Point 4', 'Point 5');
title('Position along y axis')
xlabel('Theta2 (rad)')
ylabel('Position (in)')

%% plots for output links %%
figure(8)
plot(theta2,theta3)
hold on
plot(theta2,theta4)
grid on
legend('Slotted Link','Coupler')
title('Angular position of linkages')
xlabel('Theta2 (rad)')
ylabel('Angular position (rad)')

figure(9)
plot(theta2,omega3)
hold on
plot(theta2,omega4)
grid on
legend('Slotted Link','Coupler')
title('Angular Velocity of linkages')
xlabel('Theta2 (rad)')
ylabel('Angular velocity (rad/s)')

figure(10)
plot(theta2,alpha3)
hold on
plot(theta2,alpha4)
grid on
legend('Slotted Link','Coupler')
title('Angular acceleration of linkages')
xlabel('Theta2 (rad)')
ylabel('Angular acceleration (rad/s^2)')




