With minimal changes
%% R-RRR
clear all; clc; close all
% Input data
AB=0.325; %(m)
BC=0.938; %(m)
CD=0.675; %(m)
CE=0.6; %(m)
xD=0.8; %(m)
yD=-0.432; %(m)
phi =pi; %(rad)
xA = 0; yA = 0;
rA = [xA yA 0];
rD = [xD yD 0];
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0];
% Position of joint C
%
eqnC1 = '(xCsol - xB)^2 + (yCsol - yB)^2 = BC^2 ';
% Distance formula: CD=constant
eqnC2 = '(xCsol - xD)^2 + (yCsol - yD)^2 = CD^2 ';
% Simultaneously solve above equations
solC = solve(str2sym(eqnC1), str2sym(eqnC2), sym('xCsol'), sym('yCsol'));
% Two solutions for xC - vector form
xCpositions = eval(solC.xCsol);
% Two solutions for yC - vector form
yCpositions = eval(solC.yCsol);
% Separate the solutions in scalar form
% first component of the vector xCpositions
xC1 = xCpositions(1);
% second component of the vector xCpositions
xC2 = xCpositions(2);
yC1 = yCpositions(1);
% second component of the vector yCpositions
yC2 = yCpositions(2);
% Select the correct position for C
% for the given input angle
if xC1 > xD
xC = xC1; yC=yC1;
else
xC = xC2; yC=yC2;
end
rC = [xC yC 0]; % Position vector of C
% Position of joint E
% Distance formula: CE=constant
eqnE1='(xEsol-xC)^2+(yEsol-yC)^2=CE^2';
% Slope formula:
% E, C, and D are on the same straight line
eqnE2='(yD-yC)/(xD-xC)=(yEsol-yC)/(xEsol-xC)';
solE = solve(str2sym(eqnE1), str2sym(eqnE2), sym('xEsol'), sym('yEsol'));
xEpositions = eval(solE.xEsol);
yEpositions = eval(solE.yEsol);
xE1 = xEpositions(1); xE2 = xEpositions(2);
yE1 = yEpositions(1); yE2 = yEpositions(2);
if xE1 < xC
xE = xE1; yE=yE1;
else
xE = xE2; yE=yE2;
end
rE = [xE yE 0]; % Position vector of E
% Angles of the links with the horizontal
phi2 = atan((yB-yC)/(xB-xC));
phi3 = atan((yD-yC)/(xD-xC));
fprintf('Results \n\n')
fprintf('rA = [ %g, %g, %g ] (m)\n', rA)
fprintf('rD = [ %g, %g, %g ] (m)\n', rD)
fprintf('rB = [ %g, %g, %g ] (m)\n', rB)
fprintf('rC = [ %g, %g, %g ] (m)\n', rC)
fprintf('rE = [ %g, %g, %g ] (m)\n', rE)
fprintf('phi2 = %g (degrees) \n', phi2*180/pi)
fprintf('phi3 = %g (degrees) \n', phi3*180/pi)
% Graphic of the mechanism
plot([xA,xB],[yA,yB],'k-o','LineWidth',1.5)
hold on % holds the current plot
plot([xB,xC],[yB,yC],'b-o','LineWidth',1.5)
hold on
plot([xD,xE],[yD,yE],'r-o','LineWidth',1.5)
% adds major grid lines to the current axes
grid on,...
xlabel('x (m)'), ylabel('y (m)'),...
title('positions for \phi = 45 (deg)'),...
text(xA,yA,'\leftarrow A = ground',...
'HorizontalAlignment','left'),...
text(xB,yB,' B'),...
text(xC,yC,'\leftarrow C = ground',...
'HorizontalAlignment','left'),...
text(xD,yD,'\leftarrow D = ground',...
'HorizontalAlignment','left'),...
text(xE,yE,' E'), axis([-2 3 -2 3])
