clear all; close all
t=linspace(0,2*pi/50,100); % create time varying from 0-0.126 s (or one period)
x=20*sin(50*t);dx=20*50*cos(50*t); % define x and dx/dt in terms of time (note dx=dx/dt)
t1=0.2*pi*cos(50*t); dt1=-10*pi*sin(50*t); % define theta1 (t1) and dtheta1/dt
t2=0.2*pi*sin(50*t-pi/3); dt2=10*pi*sin(50*t-pi/3); % define theta2 (t2) and dtheta2/dt;
L1=1;L2=1.5; % set lengths for L1 and L2 (none were given in problem so 1 and 1.5 mm were
             % chosen arbitrarily
rcc=[x+L1*sin(t1);-L1*cos(t1)]; % position of connection between links
rco=[x+L1*sin(t1)+L2*sin(t2);-(L1*cos(t1)+L2*cos(t2))]; % create a row vectors of
                                                        % x-component and y-component of
                                                        % pendulum position C (r_C/O) 
vco=[dx+L1*cos(t1).*dt1+L2*cos(t2).*dt2;(L1*sin(t1).*dt1+L2*sin(t2).*dt2)]; % create row
                                                                            % vectors of
                                                                            % the x- and
                                                                            % y-component
                                                                            % velocity of
                                                                            % point C


figure(1)
plot(t,vco(1,:),t,vco(2,:))
xlabel('time (s)')
ylabel('velocity (mm/s)')
legend('x-component','y-component')
title('Velocity of end point')

% Now plot the position  each time step 
for i =1:length(t)
  hold on
	plot([x(i),rcc(1,i),rco(1,i)],[0,rcc(2,i),rco(2,i)]) % plot lines for 2 links
  plot(rco(1,:),rco(2,:),'--','LineWidth',2); % this plots all of the positions in the x-y-plane
                                        % over time
  plot(rco(1,i),rco(2,i),'o','MarkerSize',10); % this plots the position as a circle at
                                              % timestep i
  axis([-30 30 -5 0]) % this sets the axis bounds 
	filename=sprintf('pendulum_output/%05d.png',i);
	print(filename); % another option for saving a plot to a png is the 'print' command
  hold off 
  clf % this clears the figure for the next time step
end
% this is a system command to use ImageMagick's cli 'convert' to create an animated gif
% if you don't have ImageMagick installed, comment this next line
system ("convert -delay 10 -loop 0 pendulum_output/*png pendulum.gif")