********************************************************

%1.輸入已知數(shù)據(jù)

clear；

l1=0.4；

l3=1.6；

l4=1

omega1=10；

hd=pi/180；

du=180/pi；

J3=2.2；

G3=98； G1=1.2^*9.8；

g=9.8；

Mr=100；

m3=G3/g；　m1=G1/g；

%2.導(dǎo)桿機(jī)構(gòu)運(yùn)動分析

%…………………　計算構(gòu)件的位移及角位移…………………………………

for n1=1∶400；

　　theta1（n1）=n1^*hd；

　　s3（n1）=sqrt（（l1^*cos（theta1（n1）））^*（l1^*cos（theta1（n1）））﹢（l4﹢l1^*sin（theta1（n1）））^*（l4﹢l1^*sin（theta1（n1））））；

　　%s3表示滑塊2相對于CD桿的位移

　　theta3（n1）=acos（（l1^*cos（theta1（n1）））/s3（n1））； %theta3表示桿3轉(zhuǎn)過角度

end

%…………………　計算構(gòu)件的角速度及速度…………………………

for n1=1：400；

　　A=［sin（theta3（n1）），s3（n1）^*cos（theta3（n1））；　%從動件位置參數(shù)矩陣

　　　　-cos（theta3（n1）），s3（n1）^*sin（theta3（n1））］；

　　B=［l1^*cos（theta1（n1））；l1^*sin（theta1（n1））］；　　%原動件位置參數(shù)矩陣

　　omega=A\（omega1^*B）；

　　v2（n1）=omega（1）；　　　%滑塊2的速度

　　omega3（n1）=omega（2）；　　%構(gòu)件3的角速度

　　%…………………………　計算構(gòu)件的角加速度及加速度 …………………………

　　A=［sin（theta3（n1）），s3（n1）^*cos（theta3（n1））；　　%從動件位置參數(shù)矩陣

　　　　cos（theta3（n1）），-s3（n1）^*sin（theta3（n1））］；

　　At=［omega3（n1）^*cos（theta3（n1）），（v2（n1）^*cos（theta3（n1））-s3（n1）^*omega3（n1）^*sin（theta3（n1）））；

　　　　-omega3（n1）^*sin（theta3（n1）），（-v2（n1）^*sin（theta3（n1））-s3（n1）^*omega3（n1）^*cos（theta3（n1）））］；

　　Bt=［-l1^*omega1^*sin（theta1（n1））；-l1^*omega1^*cos（theta1（n1））］；

　　alpha=A\（-At^*omega﹢omega1^*Bt）；　%機(jī)構(gòu)從動件的加速度列陣

　　a2（n1）=alpha（1）；　　%a2表示滑塊2的加速度

　　alpha3（n1）=alpha（2）；　　%alpha3表示桿3的角加速度

end

%3.導(dǎo)桿機(jī)構(gòu)力平衡計算

for n1=1：400；

　　% 計算各個鉸鏈點坐標(biāo)

　　xa=0；

　　ya=l4；

　　xb（n1）=l1^*cos（theta1（n1））；

　　yb（n1）=l4﹢l1^*sin（theta1（n1））；

　　xc=0；

　　yc=0；

　　% 計算各個質(zhì)心點坐標(biāo)

　　xs3（n1）=l3^*cos（theta3（n1））/2；

　　ys3（n1）=l3^*sin（theta3（n1））/2；

　　% 計算各個質(zhì)心點加速度

　　a3x（n1）=-l3^*（alpha3（n1）^*sin（theta3（n1））﹢omega3（n1）^∧2^*cos（theta3（n1）））/2；

　　a3y（n1）=l3^*（alpha3（n1）^*cos（theta3（n1））-omega3（n1）^∧2^*sin（theta3（n1）））/2；

　　% 計算各構(gòu)件慣性力和慣性力矩

　　F3x（n1）=-m3^*a3x（n1）； F3y（n1）=-m3^*a3y（n1）；% 計算慣性力

　　Mf3（n1）=-J3^*alpha3（n1）；　　　　% 計算慣性力矩

　　% 未知力系數(shù)矩陣

　　C=zeros（9）；

　　C（1，1）=-1；　　　　　　　　　　C（1，3）=-1；

　　C（2，2）=-1；　　　　　　　　　　C（2，4）=-1；

　　C（3，3）=yb（n1）-ya；　　　　　　C（3，4）=xa-xb（n1）； C（3，9）=1

　　C（4，3）=1；　　　　　　　　　　C（4，5）=-1；

　　C（5，4）=1；　　　　　　　　　　C（5，6）=-1；

　　C（6，5）=cos（theta3（n1））；　　　C（6，6）=sin（theta3（n1））；

　　C（7，5）=1；　　　　　　　　　　C（7，7）=-1；

　　C（8，6）=1；　　　　　　　　　　C（8，8）=-1；

　　C（9，5）=ys3（n1）-yb（n1）；　　　　C（9，6）=xb（n1）-xs3（n1）；

　　C（9，7）=-（ys3（n1）-yc）；　　　　C（9，8）=-（xc-xs3（n1））；　　C（9，9）=0；

　　% 已知力列陣

　　D=［0；G1；0；0；0；0；-F3x（n1）；-F3y（n1）﹢G3；Mr-Mf3（n1）］；

　　% 求未知力列陣

　　FR=inv（C）^*D；

　　Fr14x（n1）=FR（1）；

　　Fr14y（n1）=FR（2）；

　　Fr12x（n1）=FR（3）；

　　Fr12y（n1）=FR（4）；

　　Fr23x（n1）=FR（5）；

　　Fr23y（n1）=FR（6）；

　　Fr34x（n1）=FR（7）；

　　Fr34y（n1）=FR（8）；

　　Mb（n1）=FR（9）；

end

%4.輸出機(jī)構(gòu)的力分析線圖

figure（1）；

n1=1：400；

　subplot（2，2，1）；　%繪運(yùn)動副反力FR12　曲線圖

　plot（n1， Fr12x，'b'）；

　hold on

　plot（n1，F(xiàn)r12y，'k'）；

　legend（'F_R_1_2_x'，'F_R_1_2_y'）

　title（'運(yùn)動副反力F_R_1_2曲線圖'）；

　xlabel（'曲柄轉(zhuǎn)角 \theta_1/\circ'）

　ylabel（'F/N'）

　grid on；

　subplot（2，2，2）；　%繪運(yùn)動副反力FR23曲線圖

　plot（n1，F(xiàn)r23x（n1），'b'）；

　hold on

　plot（n1，F(xiàn)r23y（n1），'k'）；

　hold on

　legend（'F_R_2_3_x'，'F_R_2_3_y'）

　title（'運(yùn)動副反力F_R_2_3曲線圖'）；

　xlabel（'曲柄轉(zhuǎn)角 \theta_1/\circ'）

　ylabel（'F/N'）

　grid on；

　subplot（2，2，3）；　%繪運(yùn)動副反力FR34曲線圖

　plot（n1，F(xiàn)r34x，'b'）；

　hold on

　plot（n1，F(xiàn)r34y，'k'）；

　hold on

　legend（'F_R_3_4_x'，'F_R_3_4_y'）

　title（'運(yùn)動副反力F_R_3_4曲線圖'）；

　xlabel（'曲柄轉(zhuǎn)角 \theta_1/\circ'）

　ylabel（'F/N'）

　grid on；

　subplot（2，2，4）；　%繪平衡力矩M_b曲線圖

　plot（n1，Mb）

　title（'力矩Mb圖'）

　xlabel（'曲柄轉(zhuǎn)角 \theta_1/\circ'）；

　ylabel（'M/N.m'）

　hold on；

　grid on；

　text（100，1.9^*10^∧6，'Mb'）