%《MATLAB及其在理工課程中的應用指南》(十一五規劃版)第七章例7－1－4程序d
% 四連桿運動的分析計算:求全程運動并動畫演示
% 西安電子科技大學出版社出版  陳懷琛編著 2007年7月
%
global L0 L1 L2 L3 th1 
L0=20;L1=8;L2=25; L3=20;% 輸入基線及三根桿的長度L1,L2,L3
w1=100;	%input('桿1角速度w1= (書上取100) [1/秒]');
theta1=linspace(0,2*pi,181);		%桿1每圈分為180份，間隔2度。
theta3=1;	%input('對應于theta1最小值處的theta3近似值= (書上取1) [弧度]');
dt = 2*pi/180/w1;					% 桿1轉2度對應的時間增量
th1=theta1(1);theta3(1)=fzero('ex714f',theta3);		%求初始輸出theta3
for i=2:181
   th1=theta1(i);
   theta3(i)=fzero('exn714f',theta3(i-1)); % 調用ex714f函數逐次求輸出theta3
end
figure(1)
subplot(1,2,1),plot(theta1,theta3);xlabel('theta1'),ylabel('theta3'),grid	%畫曲線
w3 = diff(theta3)/dt;		 	% 求桿3的角速度,注意求導數后數組長度小一
subplot(1,2,2),plot(theta1(2:length(theta1)),w3);grid   
% 繪制四連桿運動圖
pause,figure(2),subplot(1,1,1),axis equal
x0=10;y0=0;
x1=x0+L1*cos(theta1);y1=L1*sin(theta1);
x2=x0+L0+L3*cos(theta3);y2=L3*sin(theta3);
x3=x0+L0;y3=0;
set(gcf,'color','w')
h1=line([x0,x1(1),x2(1),x3(1)],[y0,y1(1),y2(1),y3(1)],'Linewidth',3);
for i1=1:1000
   i=mod(i1,180)+1;             % 時間不斷增長時，使下標循環
   plot([x0,x1(i),x2(i),x3(1)],[y0,y1(i),y2(i),y3(1)],'Linewidth',3);
   line([5,45],[0,0])
   axis([0,50,-20,20]),axis off
   fft(randn(2^12,1));
   drawnow
end