% 曲柄搖桿機構運動分析
% (1)-----計算連桿的輸出角th3和搖桿的輸出角th4
% 設定各桿的長度(單位:毫米)
rs(1)=304.8;        % 設定機架1長度
rs(2)=101.6;        % 設定曲柄2長度
rs(3)=254.0;        % 設定連桿3長度
rs(4)=177.8;        % 設定搖桿4長度
dr=pi/180.0;% 角度與弧度的轉換系數
% 設定初始推測的輸入
% 機構的初始位置
th(1)=0.0;          % 設定曲柄2初始位置角是0度(與機架1共線)
th(2)=45*dr;        % 連桿3的初始位置角是 45度
th(3)=135*dr;       % 搖桿4的初始位置角是135度
% 搖桿4的初始位置角可以用三角形的正弦定理確定
th(3)=pi-asin(sin(th(2))*rs(3)/rs(4))
dth=5*dr;                               % 循環增量
% 曲柄輸入角從0度變化到360度,步長為5度,計算th34
for i=1:72
    [th3,th4]=ntrps(th,rs); % 調用牛頓—辛普森方程求解機構位置解非線性方程函數文件
    % Store results in a matrix-th34,in degrees
    % 在矩陣th34中儲存結果,以度為單位;(i,:)表示第i行所有列的元素;(:,i)表示第i列所有行的元素
    th34(i,:)=[th(1)/dr th3/dr th4/dr]; % 矩陣[曲柄轉角 連桿轉角 搖桿轉角]
    th(1)=th(1)+dth;                    % 曲柄轉角遞增
    th(2)=th3;                          % 連桿轉角中間計算值
    th(3)=th4;                          % 搖桿轉角中間計算值
end
% 繪制輸出角th(2)與th(3)—輸入角th(1)的關系曲線
subplot(2,2,1)                          % 選擇第1個子窗口
plot(th34(:,1),th34(:,2),th34(:,1),th34(:,3))
axis([0 360 0 170])
grid                                    % 網格線
ylabel('從動件角位移/deg')
title('角位移線圖')
text(110,110,'搖桿4角位移')
text(50,35,'連桿3角位移')
% (2)-----計算連桿的角速度om3和搖桿的角速度om4
% Setting initial conditions
% 設置初始條件
om2=250;            % 曲柄角速度(等速輸入)
T=2*pi/om2;                             % 機構周期-曲柄旋轉1周的時間(秒)
% 曲柄輸入角從0度變化到360度,步長為5度,計算om34
for i=1:72
    ct(2)=i*dth;
    A=[-rs(3)*sin(th34(i,2)*dr)  rs(4)*sin(th34(i,3)*dr); rs(3)*cos(th34(i,2)*dr) -rs(4)*cos(th34(i,3)*dr)];
    B=[om2*rs(2)*sin(ct(2));-om2*rs(2)*cos(ct(2))];
    om=inv(A)*B;                        % 輸出角速度矩陣
    om3=om(1); 
    om4=om(2); 
    om34(i,:)=[i om3 om4];              % 矩陣[序號 連桿角速度 搖桿角速度]
    t(i)=i*T/72;
end
% 繪制連桿的角速度om3和搖桿的角速度om4—時間Times的關系曲線
subplot(2,2,2)                          % 選擇第2個子窗口
plot(t,om34(:,2),t,om34(:,3))
axis([0 0.026 -190 210])
grid                                    % 網格線
title('角速度線圖')
ylabel('從動件角速度/rad/s')
text(0.001,170,'搖桿4角速度')
text(0.013,130,'連桿3角速度')
% (3)-----計算連桿的角加速度a3和搖桿的角加速度a4
a2=0;               % 曲柄角速度是等速,角加速度a2=dom2/dt=0
% 曲柄輸入角從0度變化到360度,步長為5度,計算a34
for i=1:72
    c(2)=i*dth;
    C=[-rs(3)*sin(th34(i,2)*dr) rs(4)*sin(th34(i,3)*dr); rs(3)*cos(th34(i,2)*dr) -rs(4)*cos(th34(i,3)*dr)];
    D(1)= a2*rs(2)*sin(c(2))+om2^2*rs(2)*cos(c(2))+om34(i,2)^2*rs(3)*cos(th34(i,2)*dr)-om34(i,3)^2*rs(4)*cos(th34(i,3)*dr);
D(2)=-a2*rs(2)*cos(c(2))+om2^2*rs(2)*sin(c(2))+om34(i,2)^2*rs(3)*sin(th34(i,2)*dr)-om34(i,3)^2*rs(4)*sin(th34(i,3)*dr);
    a=inv(C)*D';                        % 輸出角加速度矩陣
    a3=a(1); 
    a4=a(2); 
    a34(i,:)=[i a3 a4];                 % 矩陣[序號 連桿角加速度 搖桿加角速度]
    t(i)=i*T/72;
end
% 繪制連桿的角加速度a3和搖桿的角加速度a4—時間Times的關系曲線
subplot(2,2,3)                          % 選擇第3個子窗口
plot(t,a34(:,2),t,a34(:,3))
axis([0 0.026 -6*1e4 8*1e4])
grid                                    % 網格線
title('角加速度線圖')
xlabel('時間/s')
ylabel('從動件加速度/rad/s^{2}')
text(0.003,6.2*1e4,'搖桿4角加速度')
text(0.010,3.3*1e4,'連桿3角加速度')
%
% 輸出1：四桿機構運動周期(0:5:360),時間,角位移,角速度,角加速度數據
disp '   曲柄轉角  連桿轉角-搖桿轉角-連桿角速度-搖桿角速度-連桿加速度-搖桿加速度'
ydcs=[th34(:,1),th34(:,2),th34(:,3),om34(:,2),om34(:,3),a34(:,2),a34(:,3)];
disp (ydcs)
% 輸出參數的數量級必須一致
%
% (4)-----運動誤差分析
% 閉環矢量方程:r2+r3-r4-r1=0
% 誤差矢量E=r2+r3-r4-r1的模是表示仿真有效程度的標量(ex和ey是誤差分量)
ex=rs(2)*cos(th34(:,1)*dth)+rs(3)*cos(th34(:,2)*dth)-rs(4)*cos(th34(:,3)*dth)-rs(1);
ey=rs(2)*sin(th34(:,2)*dth)+rs(3)*sin(th34(:,2)*dth)-rs(4)*sin(th34(:,3)*dth);
ee=norm([ex ey]);                       % 計算誤差矢量矩陣的范數(模)
%
% 輸出2：四桿機構運動周期(0:5:360),時間,X向誤差分量,Y向誤差分量
disp '   曲柄轉角  時間(秒)  X向誤差  Y向誤差'
wc=[th34(:,1),t(:),ex(:,1),ey(:,1)];
disp (wc)
fprintf (1,'  誤差矢量矩陣的模   ee = %3.4f \n',ee)
%
% 繪制均方根相容性誤差曲線
subplot(2,2,4)                          % 選擇第4個子窗口
plot(t,ex(:,1),t,ey(:,1))
axis([0 0.026 -800 600])
grid                                    % 網格線
title('均方根誤差曲線')
xlabel('時間/s')
ylabel('均方根誤差')
text(0.012,350,'X向誤差分量')
text(0.003,-600,'Y向誤差分量')