A. Appendix for MATLAB script
clear all
theta = 0:.01:14.32;
Bx = [0,0];
By = [0,0];
Bz = [0,0];
Cx = [0,0];
Cy = [0,0];
Cz = [0,0];
Ax = [0];
Ay = 4.85*sind(theta);
Az = -4.85*cosd(theta);
for i = 1:length(theta)
Ax(i) = -2.3;
x = theta(i);
syms bx by bz cx cy cz
eqn1 = bx^2 + by^2 + bz^2 == 4.1^2;
eqn2 = cx^2 + cy^2 + cz^2 == 0.85^2;
eqn3 = bx + cx == -0.7;
eqn4 = by + cy == 4.8-4.85*sind(x);
eqn5 = bz + cz == 4.85*cosd(x)-4;
eqn6 = cz/cx == tand(18);
[Sbx, Sby, Sbz, Scx, Scy, Scz] = solve(eqn1, eqn2, eqn3, eqn4, eqn5, eqn6);
format short
temp1 = vpa(Sbx);
temp2 = vpa(Sby);
temp3 = vpa(Sbz);
temp4 = vpa(Scx);
temp5 = vpa(Scy);
temp6 = vpa(Scz);
Bx(i) = temp1(1);
By(i) = temp2(1);
Bz(i) = temp3(1);
Cx(i) = temp4(1);
Cy(i) = temp5(1);
Cz(i) = temp6(1);
end
o = [0,0,0];
for i = 1:length(theta)
A = [Ax(i), Ay(i), Az(i)];
B = [(Bx(i)+Ax(i)), (By(i) + Ay(i)), (Bz(i) + Az(i))];
C = [(Cx(i) + Bx(i)+Ax(i)), (Cy(i) + By(i) + Ay(i)), (Cz(i) + Bz(i) + Az(i))];
figure(1)
vectarrow(o,A)
hold on
vectarrow(A,B)
vectarrow(B,C)
axis([-5 0 0 7.5 -5 0])
legend('Link A [input]', 'Link B', 'Link C [output]', 'location', 'southeast')
hold off
drawnow
frame = getframe(1);
im = frame2im(frame);
[ind,cm] = rgb2ind(im,256);
filename = 'ME350RProject.gif';
if i == 1
imwrite(ind,cm,filename,'gif', 'DelayTime', 0, 'Loopcount',inf);
else
imwrite(ind,cm,filename,'gif','WriteMode','append');
end
end
figure(2)
plot3(Ax, Ay, Az)
hold on
subx = Ax + Bx;
suby = Ay + By;
subz = Az + Bz;
plot3(subx, suby, subz)
subx = subx + Cx;
suby = suby + Cy;
subz = subz + Cz;
plot3(subx, suby, subz)
hold off
legend('Link A [input]', 'Link B', 'Link C [output]', 'location', 'southwest')
saveas(2, 'ME350RProject.jpg')
o = [0,0];
Cxref = sqrt(Cx.^2 + Cz.^2);
for i = 1:length(theta)
subC = sqrt(Cx(i)^2 + Cz(i)^2);
Cref = [subC, -Cy(i)];
figure(3)
vectarrow(o, Cref)
hold on
plot(Cxref, -Cy)
axis([0 2 -1 0.5])
title('Output Link')
hold off
drawnow
frame = getframe(3);
im = frame2im(frame);
[ind,cm] = rgb2ind(im,256);
filename = 'ME350RProjectDemo.gif';
if i == 1
imwrite(ind,cm,filename,'gif', 'DelayTime', 0, 'Loopcount',inf);
angle1 = abs(atand(Cy(i)/subC));
else
imwrite(ind,cm,filename,'gif','WriteMode','append');
end
end
outputangle = angle1 + abs(atand(Cy(i)/subC));