MATLAB code for the Kinematic/Dynamic Analysis

clear
clc
close all

% Link lengths (m)
a = 0.09;
b = 0.2032;
c = 0.126;

% Link angular positions (Positive Convention CCW, degrees)
Start_theta = 180;
End_theta = 270;
theta_step = 1;

theta1 = 180;
theta4 = 90;
theta2 = [Start_theta:theta_step:End_theta];

% Link 2 angular velocity (Set by the motor specs) (rad/s)
omega_motor = -12.48;

% Link 2 angular acceleration (rad/s^2)
alpha2 = 0; % 0 due to constant omega2

% Motor to Link 2 Torque and Omega2
r_in = 0.107/2; % m
r_out = 0.057/2; % m
omega2 = (-r_in*omega_motor)/r_out; % (Greartisan DC 12V 120 RPM Gear Motor)
Tau_motor = 6.37; % N*m (Greartisan DC 12V 120 RPM Gear Motor)
Tau2 = (r_out*Tau_motor)/(r_in);

% Spring Constant (N/m)
k = 288;

theta3 = asind((-a*sind(theta2) - c)/(b));
d = a*cosd(theta2) + b*cosd(theta3);

omega3 = (-a.*omega2.*cosd(theta2))./(b.*cosd(theta3));
d_dot = -a.*omega2.*sind(theta2) - b.*omega3.*sind(theta3); % Sliding velocity of block (m/s)

alpha3 = (a.*(omega2.^2).*sind(theta2) + b.*(omega3.^2).*sind(theta3))./(b.*cosd(theta3));
d_double_dot = -a.*(omega2.^2).*cosd(theta2) - b.* alpha3.*sind(theta3) - b.*(omega3.^2).*cosd(theta3); % Sliding acceleration

% Force output along the x component (Sliding axis)
v_in = omega2*a;
F_in = Tau2/a;
F_out_x = (F_in.*v_in)./d_dot;
delta_x = d - d(1); % (m)
F_s = k*delta_x;

F_resultant = F_out_x - F_s; % Units should be (kg*m/s^2)

% Plotting Sliding Block Kinematics

figure_size_width = 750;
figure_size_height = 750;
figure_dist_bottom = 1000;

figure(1)
%set(gcf,'position',[0 figure_dist_bottom figure_size_width figure_size_height])
subplot(3,1,1)
plot(theta2,d)
xlabel("Theta 2 (Degrees)")
ylabel("d(mm)")
title("Horizontal Displacement of Sliding Block from Origin Vs. Theta2 ")
grid on

subplot(3,1,2)
plot(theta2,d_dot)
xlabel("Theta 2 (Degrees)")
ylabel("Sliding veloctiy of block (mm/s)")
title("Sliding Velocity Vs. Theta 2")
grid on

subplot(3,1,3)
plot(theta2,d_double_dot)
xlabel("Theta 2 (Degrees)")
ylabel("Sliding Acceleration of block (mm/s^2)")
title("Sliding Acceleration Vs. Theta 2")
grid on

figure(2)
%set(gcf,'position',[750 figure_dist_bottom figure_size_width figure_size_height])
subplot(3,1,1)
plot(theta2,F_out_x)
xlabel("Theta 2 (Degrees)")
ylabel("Sliding Force of block ")
title("Sliding Force Vs. Theta 2")
grid on

subplot(3,1,2)
plot(theta2,F_s)
xlabel("Theta 2 (Degrees)")
ylabel("Spring force at block ")
title("Spring Force Vs. Theta 2")
grid on

subplot(3,1,3)
plot(theta2,F_resultant)
xlabel("Theta 2 (Degrees)")
ylabel("Resultant force at block ")
title("Sliding Force w/ Force Spring Dynamics Vs. Theta 2")
grid on

figure(4)
plot(theta2,F_out_x,'r-',theta2,F_s,'b-');
legend("Sliding Force [N]","Spring Force [N]")
title("Sliding force vs Spring force");
xlabel("Theta 2 [deg]");
ylabel("Forces [N]");
grid on;
% Animation
% video_record = true;
%
% if video_record == true
% vid_name = 'Fourbar_Animation.avi';
% video = VideoWriter(vid_name); %create the video object
% open(video);
% end

a =a*1000;
b = b*1000;
c = c*1000;
count = 1;
for theta2_Anim = Start_theta:theta_step:End_theta

Bx = a*(cosd(theta2_Anim));
By = a*(sind(theta2_Anim));
theta3_Anim = asind((-a*sind(theta2_Anim) - c)/(b));
d_Anim = a*cosd(theta2_Anim) + b*cosd(theta3_Anim);

figure(3)

set(gcf,'position',[100 0 figure_size_width figure_size_height])
plot([0 Bx],[0 By],"-o","color","black","linewidth",3)
hold on
plot([Bx d_Anim],[By -c],"-o","color","black","linewidth",3)
plot([60 250],[-141 -141],"--","color","#808080","linewidth",2)
plot([60 250],[-111 -111],"--","color","#808080","linewidth",2)

% Draw Sliding Block
box_width = 30;
box_height = 30;
x1 = d_Anim - box_width/2;
x2 = d_Anim + box_width/2;
y1 = -c - box_height/2;
y2 = -c + box_height/2;
% Draw the Box from point (x1,y1) to (x2,y1) to (x2,y2) to (x1,y2) and
% finally back to (x1,y1)
box_x = [x1, x2, x2, x1, x1];
box_y = [y1, y1, y2, y2, y1];
plot(box_x, box_y, 'b-', 'LineWidth', 3, "color", "black");

xlim([-100 275])
ylim([-187.5 187.5])

title("Fourbar Kinematics Animation")
xlabel("X axis (m)")
ylabel("Y axis (m)")
grid on

if (count == 1)
pause(2);
end
count = count + 1;

hold off
% if video_record == true
% frame = getframe(gcf);
% writeVideo(video,frame); %write the image to file
% end
end
% close(video)