1、應用C/C+語言分析平面六連桿機構和尖底直動從動件盤狀凸輪山東大學09級機電班小組成員： 李圣 200900162096蘇海北 200900162164張海斌 200900162254鄧鴻飛 200900162030小組分工：平面六連桿機構：李圣： 六連桿機構分析，單桿運動分析（AB桿），RPR II級組運動分析（CD桿），RRP II級組運動分析（DE桿），程序設計、編寫與調試，書面報告結果校對。蘇海北：RRP II級組運動分析（DE桿），書面報告。張海斌：RRP II級組運動分析（DE桿），結果校對。鄧鴻飛：結果校對。尖底直動從動件盤狀凸輪李圣： 凸輪機構分析，程序設計、編寫與調試，書面報

2、告，結果校對。蘇海北：書面報告。張海斌：結果校對。鄧鴻飛：結果校對。說明：除六連桿機構受力分析程序中高斯法解線性方程部分參考了網絡上他人的寫法，其他程序所有部分都為自己設計、編寫、調試。沒有使用任何他人的子程序。項目過程中我付出了極大的精力和時間，自學了許多東西。編寫調試環境為VC+6.0帶模擬BDI繪圖庫。如果需要調試源代碼，請安裝文件夾下的graphics模擬庫安裝包。目錄平面六連桿機構l 六連桿運動數據文本輸出程序5l 六連桿運動數據文本輸出程序分析結果7l 六連桿運動線圖程序14l 六連桿運動線圖程序分析結果21l 六連桿受力數據文本輸出程序24l 六連桿受力數據文本輸出程序分析結果2

3、9l 六連桿受力線圖程序分34l 六連桿受力線圖程序分析結果46尖底直動從動件盤狀凸輪l 凸輪數據文本輸出程序49l 凸輪數據文本輸出程序分析結果51l 凸輪輪廓和線圖程序53l 凸輪輪廓和線圖程序分析結果58注意：1、六連桿運動屏顯程序、六連桿受力屏顯程序、凸輪數據屏顯程序沒有打印出來。2、由于各文本輸出程序分析結果數據量過大，程序中使用5度為分度，打印時用了15度。平面六連桿機構題目：如圖所示的六連桿機構中，已知l1=0.12 m, l3=l4=0.6 m, l6=0.38 m;曲柄 1 逆時針方向等速轉動，轉速n1=172 r/min;構件質量m3=20 kg, m4=15 kg, m5

4、=62 kg,構件1、2的質量忽略不計；質心位置lCS3=0.3 m, lDS4=0.3 m,質心S5在點E；構件3、4繞質心的轉動慣量JS3=0.11 kgm2；該構件在工作行程時滑塊5受生產阻力Fx=110 N。 求：1）曲柄回轉一周過程中，滑塊5的位移SE、速度E、加速度aE及構件3、4的角速度3、4，角加速度3、4；2）各運動副中的反力及應加在原動件AB上的平衡力矩Mb。六連桿運動數據文本輸出程序源代碼59#include#include #include #includeconst double PI=3.14159265358979;int main(void) double se

5、ta;double n1;double l1,l3,l4,l6; double phi,w1,alf1,xb,yb,vbx,vby,abx,aby,vb,ab;double phi1,w3,alf3,xd,yd,vdx,vdy,vr,adx,ady,e1,f1,vd,ad;double phi2,w4,alf4,xe,ye,vex,vey,aex,aey,e2,f2,f3,ve,ae;l1=0.12; l3=0.6; l4=0.6; l6=0.38; n1=172; w1=n1/30*PI;printf(六連桿運動分析nnn);FILE *fp;if(fp=fopen(運動數據.txt,w)=

6、NULL)printf(Cant open this file.n);exit(0);fprintf(fp,六連桿運動分析nnn);for(seta=0;seta90&seta=270)phi1=atan(yb+l6)/xb)+PI; elsephi1=atan(yb+l6)/xb);xd=l3*cos(phi1);yd=-l6+l3*sin(phi1);w3=(vby*cos(phi1)-vbx*sin(phi1)/(xb*cos(phi1)+(yb+l6)*sin(phi1);vdx=-w3*(yd+l6);vdy=w3*xd;vr=(vby*(yb+l6)+vbx*xb)/(xb*cos

7、(phi1)+(yb+l6)*sin(phi1);e1=abx+w3*w3*xb+2*w3*vr*sin(phi1);f1=aby+w3*w3*(yb+l6)-2*w3*vr*cos(phi1);alf3=(f1*cos(phi1)-e1*sin(phi1)/(xb*cos(phi1)+(yb+l6)*sin(phi1);adx=-alf3*(yd+l6)-w3*w3*xd; ady=alf3*xd-w3*w3*(yd+l6);vd=sqrt(vdx*vdx+vdy*vdy);ad=sqrt(adx*adx+ady*ady);e2=-2*xd;f2=xd*xd+yd*yd-l4*l4;xe=(

8、-e2+sqrt(e2*e2-4*f2)/2; phi2=atan(-yd)/(xe-xd);w4=(-vdy)/(xe-xd);vex=vdx-l4*w4*sin(phi2);f3=-ady-w4*w4*yd;alf4=f3/(xe-xd);aex=adx+alf4*yd-w4*w4*(xe-xd);aey=0;vey=0;ye=0;ve=-sqrt(vex*vex+vey*vey);ae=-sqrt(aex*aex+aey*aey);fprintf(fp,n曲柄1與x軸夾角:%.0f度n,seta);fprintf(fp, W1=%f alf1=%fn xb=%f yb=%fn vbx=%

9、f vby=%f vb=%fn abx=%f aby=%f ab=%fn,w1,alf1,xb,yb,vbx,vby,vb,abx,aby,ab);fprintf(fp, W3=%f alf3=%fn xd=%f yd=%fn vdx=%f vdy=%f vd=%fn adx=%f ady=%f ad=%fn,w3,alf3,xd,yd,vdx,vdy,vd,adx,ady,ad);fprintf(fp, W4=%f alf4=%fn xe=%f ye=%fn vex=%f vey=%f ve=%fn aex=%f aey=%f ae=%fn,w4,alf4,xe,ye,vex,vey,ve,

10、aex,aey,ae);printf(nnn*數據已成功輸入本目錄下“運動數據.txt/”*nnn);printf(nnn小組成員：李圣 200900162096n); printf( 蘇海北 200900162164n); printf( 張海斌 200900162254n); printf( 鄧鴻飛 200900162030n);printf(n程序：李圣nnn);printf(請按回車結束);fflush(stdin); getchar();return 0;運動分析數據結果六連桿運動分析曲柄1與x軸夾角:0度 W1=18.011798 alf1=0.000000 xb=0.120000

11、 yb=0.000000 vbx=0.000000 vby=2.161416 vb=2.161416 abx=-38.930984 aby=0.000000 ab=38.930984 W3=1.633312 alf3=76.264307 xd=0.180679 yd=0.192150 vdx=-0.934499 vdy=0.295105 vd=0.979987 adx=-44.116591 ady=12.253018 ad=45.786570 W4=-0.519185 alf4=-21.648157 xe=0.749079 ye=0.000000 vex=-1.034260 vey=0.000

12、000 ve=-1.034260 aex=-48.429490 aey=0.000000 ae=-48.429490曲柄1與x軸夾角:15度 W1=18.011798 alf1=0.000000 xb=0.115911 yb=0.031058 vbx=-0.559416 vby=2.087767 vb=2.161416 abx=-37.604442 aby=-10.076080 ab=38.930984 W3=2.587372 alf3=55.833644 xd=0.162839 yd=0.197480 vdx=-1.494156 vdy=0.421325 vd=1.552423 adx=-3

13、3.332951 ady=5.225963 ad=33.740130 W4=-0.743642 alf4=-9.416613 xe=0.729409 ye=0.000000 vex=-1.641011 vey=0.000000 ve=-1.641011 aex=-35.505862 aey=0.000000 ae=-35.505862曲柄1與x軸夾角:30度 W1=18.011798 alf1=0.000000 xb=0.103923 yb=0.060000 vbx=-1.080708 vby=1.871841 vb=2.161416 abx=-33.715221 aby=-19.465492

14、 ab=38.930984 W3=3.278077 alf3=39.864945 xd=0.137919 yd=0.203934 vdx=-1.914179 vdy=0.452108 vd=1.966846 adx=-24.760525 ady=-0.776711 ad=24.772705 W4=-0.801212 alf4=1.144464 xe=0.702198 ye=0.000000 vex=-2.077573 vey=0.000000 ve=-2.077573 aex=-24.889365 aey=0.000000 ae=-24.889365曲柄1與x軸夾角:45度 W1=18.011

15、798 alf1=0.000000 xb=0.084853 yb=0.084853 vbx=-1.528352 vby=1.528352 vb=2.161416 abx=-27.528362 aby=-27.528362 ab=38.930984 W3=3.762598 alf3=27.275707 xd=0.107742 yd=0.210247 vdx=-2.220863 vdy=0.405390 vd=2.257559 adx=-17.624726 ady=-5.417478 ad=18.438547 W4=-0.721388 alf4=9.445671 xe=0.669699 ye=0.

16、000000 vex=-2.372533 vey=0.000000 ve=-2.372533 aex=-15.931244 aey=0.000000 ae=-15.931244曲柄1與x軸夾角:60度 W1=18.011798 alf1=0.000000 xb=0.060000 yb=0.103923 vbx=-1.871841 vby=1.080708 vb=2.161416 abx=-19.465492 aby=-33.715221 ab=38.930984 W3=4.082190 alf3=17.007330 xd=0.073827 yd=0.215441 vdx=-2.430702 v

17、dy=0.301375 vd=2.449314 adx=-11.357125 ady=-8.666995 ad=14.286395 W4=-0.538182 alf4=15.365708 xe=0.633814 ye=0.000000 vex=-2.546649 vey=0.000000 ve=-2.546649 aex=-8.208921 aey=0.000000 ae=-8.208921曲柄1與x軸夾角:75度 W1=18.011798 alf1=0.000000 xb=0.031058 yb=0.115911 vbx=-2.087767 vby=0.559416 vb=2.161416

18、abx=-10.076080 aby=-37.604442 ab=38.930984 W3=4.263887 alf3=8.165880 xd=0.037504 yd=0.218827 vdx=-2.553330 vdy=0.159912 vd=2.558332 adx=-5.571793 ady=-10.580858 ad=11.958237 W4=-0.286235 alf4=18.907197 xe=0.596176 ye=0.000000 vex=-2.615966 vey=0.000000 ve=-2.615966 aex=-1.480165 aey=0.000000 ae=-1.4

19、80165曲柄1與x軸夾角:90度 W1=18.011798 alf1=0.000000 xb=0.000000 yb=0.120000 vbx=-2.161416 vby=0.000000 vb=2.161416 abx=-0.000000 aby=-38.930984 ab=38.930984 W3=4.322831 alf3=0.000000 xd=0.000000 yd=0.220000 vdx=-2.593699 vdy=0.000000 vd=2.593699 adx=-0.000000 ady=-11.212123 ad=11.212123 W4=-0.000000 alf4=2

20、0.085800 xe=0.558211 ye=0.000000 vex=-2.593699 vey=0.000000 ve=-2.593699 aex=4.418876 aey=0.000000 ae=-4.418876曲柄1與x軸夾角:105度 W1=18.011798 alf1=0.000000 xb=-0.031058 yb=0.115911 vbx=-2.087767 vby=-0.559416 vb=2.161416 abx=10.076080 aby=-37.604442 ab=38.930984 W3=4.263887 alf3=-8.165880 xd=-0.037504 y

21、d=0.218827 vdx=-2.553330 vdy=-0.159912 vd=2.558332 adx=5.571793 ady=-10.580858 ad=11.958237 W4=0.286235 alf4=18.907197 xe=0.521169 ye=0.000000 vex=-2.490694 vey=0.000000 ve=-2.490694 aex=9.663421 aey=0.000000 ae=-9.663421曲柄1與x軸夾角:120度 W1=18.011798 alf1=0.000000 xb=-0.060000 yb=0.103923 vbx=-1.871841

22、 vby=-1.080708 vb=2.161416 abx=19.465492 aby=-33.715221 ab=38.930984 W3=4.082190 alf3=-17.007330 xd=-0.073827 yd=0.215441 vdx=-2.430702 vdy=-0.301375 vd=2.449314 adx=11.357125 ady=-8.666995 ad=14.286395 W4=0.538182 alf4=15.365708 xe=0.486160 ye=0.000000 vex=-2.314756 vey=0.000000 ve=-2.314756 aex=14

23、.505330 aey=0.000000 ae=-14.505330曲柄1與x軸夾角:135度 W1=18.011798 alf1=0.000000 xb=-0.084853 yb=0.084853 vbx=-1.528352 vby=-1.528352 vb=2.161416 abx=27.528362 aby=-27.528362 ab=38.930984 W3=3.762598 alf3=-27.275707 xd=-0.107742 yd=0.210247 vdx=-2.220863 vdy=-0.405390 vd=2.257559 adx=17.624726 ady=-5.4174

24、78 ad=18.438547 W4=0.721388 alf4=9.445671 xe=0.454216 ye=0.000000 vex=-2.069193 vey=0.000000 ve=-2.069193 aex=19.318208 aey=0.000000 ae=-19.318208曲柄1與x軸夾角:150度 W1=18.011798 alf1=0.000000 xb=-0.103923 yb=0.060000 vbx=-1.080708 vby=-1.871841 vb=2.161416 abx=33.715221 aby=-19.465492 ab=38.930984 W3=3.2

25、78077 alf3=-39.864945 xd=-0.137919 yd=0.203934 vdx=-1.914179 vdy=-0.452108 vd=1.966846 adx=24.760525 ady=-0.776711 ad=24.772705 W4=0.801212 alf4=1.144464 xe=0.426361 ye=0.000000 vex=-1.750785 vey=0.000000 ve=-1.750785 aex=24.631686 aey=0.000000 ae=-24.631686曲柄1與x軸夾角:165度 W1=18.011798 alf1=0.000000 x

26、b=-0.115911 yb=0.031058 vbx=-0.559416 vby=-2.087767 vb=2.161416 abx=37.604442 aby=-10.076080 ab=38.930984 W3=2.587372 alf3=-55.833644 xd=-0.162839 yd=0.197480 vdx=-1.494156 vdy=-0.421325 vd=1.552423 adx=33.332951 ady=5.225963 ad=33.740130 W4=0.743642 alf4=-9.416613 xe=0.403731 ye=0.000000 vex=-1.347

27、302 vey=0.000000 ve=-1.347302 aex=31.160041 aey=0.000000 ae=-31.160041曲柄1與x軸夾角:180度 W1=18.011798 alf1=0.000000 xb=-0.120000 yb=0.000000 vbx=-0.000000 vby=-2.161416 vb=2.161416 abx=38.930984 aby=-0.000000 ab=38.930984 W3=1.633312 alf3=-76.264307 xd=-0.180679 yd=0.192150 vdx=-0.934499 vdy=-0.295105 vd

28、=0.979987 adx=44.116591 ady=12.253018 ad=45.786570 W4=0.519185 alf4=-21.648157 xe=0.387721 ye=0.000000 vex=-0.834737 vey=0.000000 ve=-0.834737 aex=39.803692 aey=0.000000 ae=-39.803692曲柄1與x軸夾角:195度 W1=18.011798 alf1=0.000000 xb=-0.115911 yb=-0.031058 vbx=0.559416 vby=-2.087767 vb=2.161416 abx=37.6044

29、42 aby=10.076080 ab=38.930984 W3=0.346106 alf3=-101.634306 xd=-0.189145 yd=0.189407 vdx=-0.197075 vdy=-0.065464 vd=0.207663 adx=57.893929 ady=19.155411 ad=60.980626 W4=0.114987 alf4=-33.650533 xe=0.380175 ye=0.000000 vex=-0.175296 vey=0.000000 ve=-0.175296 aex=51.512759 aey=0.000000 ae=-51.512759曲柄1

30、與x軸夾角:210度 W1=18.011798 alf1=0.000000 xb=-0.103923 yb=-0.060000 vbx=1.080708 vby=-1.871841 vb=2.161416 abx=33.715221 aby=19.465492 ab=38.930984 W3=-1.336565 alf3=-129.975083 xd=-0.185328 yd=0.190661 vdx=0.762725 vdy=0.247702 vd=0.801939 adx=74.502750 ady=23.068529 ad=77.992415 W4=-0.435405 alf4=-40.

31、612812 xe=0.383574 ye=0.000000 vex=0.679710 vey=0.000000 ve=-0.679710 aex=66.651630 aey=0.000000 ae=-66.651630曲柄1與x軸夾角:225度 W1=18.011798 alf1=0.000000 xb=-0.084853 yb=-0.084853 vbx=1.528352 vby=-1.528352 vb=2.161416 abx=27.528362 aby=27.528362 ab=38.930984 W3=-3.407883 alf3=-152.888455 xd=-0.165781

32、yd=0.196643 vdx=1.965130 vdy=0.564962 vd=2.044730 adx=90.087323 ady=18.649043 ad=91.997351 W4=-0.996649 alf4=-33.243354 xe=0.401080 ye=0.000000 vex=1.769147 vey=0.000000 ve=-1.769147 aex=82.987194 aey=0.000000 ae=-82.987194曲柄1與x軸夾角:240度 W1=18.011798 alf1=0.000000 xb=-0.060000 yb=-0.103923 vbx=1.8718

33、41 vby=-1.080708 vb=2.161416 abx=19.465492 aby=33.715221 ab=38.930984 W3=-5.661968 alf3=-150.933411 xd=-0.127424 yd=0.206313 vdx=3.319686 vdy=0.721470 vd=3.397181 adx=92.579189 ady=0.436562 ad=92.580218 W4=-1.280533 alf4=-1.375307 xe=0.435990 ye=0.000000 vex=3.055495 vey=0.000000 ve=-3.055495 aex=91

34、.371579 aey=0.000000 ae=-91.371579曲柄1與x軸夾角:255度 W1=18.011798 alf1=0.000000 xb=-0.031058 yb=-0.115911 vbx=2.087767 vby=-0.559416 vb=2.161416 abx=10.076080 aby=37.604442 ab=38.930984 W3=-7.551974 alf3=-99.560438 xd=-0.070080 yd=0.215893 vdx=4.500170 vdy=0.529244 vd=4.531184 adx=63.324232 ady=-27.00794

35、5 ad=68.843209 W4=-0.945396 alf4=47.899935 xe=0.489732 ye=0.000000 vex=4.296066 vey=0.000000 ve=-4.296066 aex=73.165159 aey=0.000000 ae=-73.165159曲柄1與x軸夾角:270度 W1=18.011798 alf1=0.000000 xb=-0.000000 yb=-0.120000 vbx=2.161416 vby=-0.000000 vb=2.161416 abx=0.000000 aby=38.930984 ab=38.930984 W3=-8.31

36、3137 alf3=-0.000000 xd=0.000000 yd=0.220000 vdx=4.987882 vdy=-0.000000 vd=4.987882 adx=0.000000 ady=-41.464953 ad=41.464953 W4=0.000000 alf4=74.281806 xe=0.558211 ye=0.000000 vex=4.987882 vey=0.000000 ve=-4.987882 aex=16.341997 aey=0.000000 ae=-16.341997曲柄1與x軸夾角:285度 W1=18.011798 alf1=0.000000 xb=0.

37、031058 yb=-0.115911 vbx=2.087767 vby=0.559416 vb=2.161416 abx=-10.076080 aby=37.604442 ab=38.930984 W3=-7.551974 alf3=99.560438 xd=0.070080 yd=0.215893 vdx=4.500170 vdy=-0.529244 vd=4.531184 adx=-63.324232 ady=-27.007945 ad=68.843209 W4=0.945396 alf4=47.899935 xe=0.629893 ye=0.000000 vex=4.704275 ve

38、y=0.000000 ve=-4.704275 aex=-53.483305 aey=0.000000 ae=-53.483305曲柄1與x軸夾角:300度 W1=18.011798 alf1=0.000000 xb=0.060000 yb=-0.103923 vbx=1.871841 vby=1.080708 vb=2.161416 abx=-19.465492 aby=33.715221 ab=38.930984 W3=-5.661968 alf3=150.933411 xd=0.127424 yd=0.206313 vdx=3.319686 vdy=-0.721470 vd=3.3971

39、81 adx=-92.579189 ady=0.436562 ad=92.580218 W4=1.280533 alf4=-1.375307 xe=0.690837 ye=0.000000 vex=3.583877 vey=0.000000 ve=-3.583877 aex=-93.786799 aey=0.000000 ae=-93.786799曲柄1與x軸夾角:315度 W1=18.011798 alf1=0.000000 xb=0.084853 yb=-0.084853 vbx=1.528352 vby=1.528352 vb=2.161416 abx=-27.528362 aby=27

40、.528362 ab=38.930984 W3=-3.407883 alf3=152.888455 xd=0.165781 yd=0.196643 vdx=1.965130 vdy=-0.564962 vd=2.044730 adx=-90.087323 ady=18.649043 ad=91.997351 W4=0.996649 alf4=-33.243354 xe=0.732642 ye=0.000000 vex=2.161114 vey=0.000000 ve=-2.161114 aex=-97.187452 aey=0.000000 ae=-97.187452曲柄1與x軸夾角:330度

41、 W1=18.011798 alf1=0.000000 xb=0.103923 yb=-0.060000 vbx=1.080708 vby=1.871841 vb=2.161416 abx=-33.715221 aby=19.465492 ab=38.930984 W3=-1.336565 alf3=129.975083 xd=0.185328 yd=0.190661 vdx=0.762725 vdy=-0.247702 vd=0.801939 adx=-74.502750 ady=23.068529 ad=77.992415 W4=0.435405 alf4=-40.612812 xe=0.

42、754229 ye=0.000000 vex=0.845740 vey=0.000000 ve=-0.845740 aex=-82.353871 aey=0.000000 ae=-82.353871曲柄1與x軸夾角:345度 W1=18.011798 alf1=0.000000 xb=0.115911 yb=-0.031058 vbx=0.559416 vby=2.087767 vb=2.161416 abx=-37.604442 aby=10.076080 ab=38.930984 W3=0.346106 alf3=101.634306 xd=0.189145 yd=0.189407 vdx

43、=-0.197075 vdy=0.065464 vd=0.207663 adx=-57.893929 ady=19.155411 ad=60.980626 W4=-0.114987 alf4=-33.650533 xe=0.758465 ye=0.000000 vex=-0.218854 vey=0.000000 ve=-0.218854 aex=-64.275098 aey=0.000000 ae=-64.275098曲柄1與x軸夾角:360度 W1=18.011798 alf1=0.000000 xb=0.120000 yb=-0.000000 vbx=0.000000 vby=2.161

44、416 vb=2.161416 abx=-38.930984 aby=0.000000 ab=38.930984 W3=1.633312 alf3=76.264307 xd=0.180679 yd=0.192150 vdx=-0.934499 vdy=0.295105 vd=0.979987 adx=-44.116591 ady=12.253018 ad=45.786570 W4=-0.519185 alf4=-21.648157 xe=0.749079 ye=0.000000 vex=-1.034260 vey=0.000000 ve=-1.034260 aex=-48.429490 aey

45、=0.000000 ae=-48.429490六連桿運動線圖程序源代碼：六連桿運動線圖#include#include#include #include const double PI=3.14159265358979;void drawst();void drawvb(double y361); void drawab(double y361);void drawvd(double y361); void drawad(double y361);void drawve(double y361); void drawae(double y361);int main(void) int j;do

46、uble seta;double n1;double l1,l3,l4,l6; double phi,w1,alf1,xb,yb,vbx,vby,abx,aby,vb,ab;double phi1,w3,alf3,xd,yd,vdx,vdy,vr,adx,ady,e1,f1,vd,ad;double phi2,w4,alf4,xe,ye,vex,vey,aex,aey,e2,f2,f3,ve,ae;double vvb361,aab361,vvd361,aad361,vve361,aae361;j=0;l1=0.12; l3=0.6; l4=0.6; l6=0.38; n1=172; w1=n

47、1/30*PI;for(seta=0;seta90&seta=270)phi1=atan(yb+l6)/xb)+PI; elsephi1=atan(yb+l6)/xb);xd=l3*cos(phi1);yd=-l6+l3*sin(phi1);w3=(vby*cos(phi1)-vbx*sin(phi1)/(xb*cos(phi1)+(yb+l6)*sin(phi1);vdx=-w3*(yd+l6);vdy=w3*xd;vr=(vby*(yb+l6)+vbx*xb)/(xb*cos(phi1)+(yb+l6)*sin(phi1);e1=abx+w3*w3*xb+2*w3*vr*sin(phi1)

48、;f1=aby+w3*w3*(yb+l6)-2*w3*vr*cos(phi1);alf3=(f1*cos(phi1)-e1*sin(phi1)/(xb*cos(phi1)+(yb+l6)*sin(phi1);adx=-alf3*(yd+l6)-w3*w3*xd; ady=alf3*xd-w3*w3*(yd+l6);vd=sqrt(vdx*vdx+vdy*vdy);ad=sqrt(adx*adx+ady*ady);e2=-2*xd;f2=xd*xd+yd*yd-l4*l4;xe=(-e2+sqrt(e2*e2-4*f2)/2; phi2=atan(-yd)/(xe-xd);w4=(-vdy)/(xe-xd);vex=vdx-l4*w4*sin(phi2);f3=-ady-w4*w4*yd;alf4=f3/(xe-xd);aex=adx+alf4*yd-w4*w4*(xe-xd);aey=0;vey=