[xy3]=serialplot(j_vars, base, lengths, i)

% function for plotting each serial link
function [xy3]=serialplot(j_vars, base, lengths, i)
global angles L
x1=j_vars(1,1)*L(i); % distance traversed along prismatic joint
th1=j_vars(1,2); % link angles in absolute coordinates
th2=j_vars(1,3);
l1=lengths(1); % link lengths
l2=lengths(2);
x(1)=base(1);
y(1)=base(2);
x(2)=x(1)+x1*cos(angles(i));
y(2)=y(1)+x1*sin(angles(i));
x(3)=x(2)+l1*cos(th1);
y(3)=y(2)+l1*sin(th1);
x(4)=x(3)+l2*cos(th2);
y(4)=y(3)+l2*sin(th2);
xy3=[x(3) y(3)];
if i==1 % plot in various colors depending on link id
    plot(x,y,'color','r');
elseif i==2
    plot(x,y,'color','g');
elseif i==3
    plot(x,y,'color','b');
else
    plot(x,y,'color','k');
end
return