牛头刨床程序设计

#include"stdio.h"#include"stdlib.h"#include"math.h"constdoublePI=3.14159;doubleL[10];doubleX[10],Y[10];doubleV[10],U[10];doubleA[10],B[10];doubleF[10],W[10],E[10];doubleS[10],C[10];doubleSgn(doubleXin){doubleResf;if(Xin>=0)Resf=1.0;if(Xin<0)Resf=-1.0;returnResf;}doubleAngle(doubleXin,doubleYin){doubleResf;if(fabs(Xin)>1e-10){Resf=atan(Yin/Xin);Resf=Resf-(Sgn(Xin)-1)*PI/2;}else{Resf=PI/2;Resf=Resf-(Sgn(Yin)-1)*Resf;}return(Resf);}voidmcrank(inti,intj,inta,intb,doubleF9){F[j]=F[j]+F9;/*杆的转角*/S[i]=L[i]*sin(F[j]);/*杆长在x,y轴上投影长度*/C[i]=L[i]*cos(F[j]);X[b]=X[a]+C[i];/*B点的x,y轴上坐标*/Y[b]=Y[a]+S[i];V[b]=V[a]-W[j]*S[i]; /*B点在x,y方向上速度*/U[b]=U[a]+W[j]*C[i];A[b]=A[a]-W[j]*W[j]*C[i]-E[j]*S[i];/*B点在x,y方向上加速度*/B[b]=B[a]-W[j]*W[j]*S[i]+E[j]*S[i];}intmrpr(inti,intj,intk,intb,intc,intd,inte,intm,doubleRes[3]){doubleA0,B0,C0,X1,Y1,F1,Ar,Ak;doubleG1,G4,G5,G6,s1,v1,a1;A0=X[b]-X[d];B0=Y[b]-Y[d];C0=L[i]+L[k];G1=A0*A0+B0*B0-C0*C0;if(G1<0)return(0);s1=sqrt(G1);X1=C0-B0;Y1=A0+m*s1;F1=Angle(X1,Y1);if(F1<PI) F[j]=2*(F1+Sgn(X1)*PI/2);if(F1>PI||F1<0)F[j]=2*(F1+Sgn(X1)*PI);if(fabs(F1)<0.001)F[j]=2*PI;S[i]=L[i]*sin(F[j]);C[i]=L[i]*cos(F[j]);S[k]=L[k]*sin(F[j]);C[k]=L[k]*cos(F[j]);S[j]=L[j]*sin(F[j]);C[j]=L[j]*cos(F[j]);X[c]=X[b]-S[i];Y[c]=Y[b]+C[i];X[e]=X[c]+C[j]-s1*cos(F[j]);Y[e]=Y[c]+S[j]-s1*sin(F[j]);G6=(X[b]-X[d])*cos(F[j])+(Y[b]-Y[d])*sin(F[j]);W[j]=((U[b]-U[d])*cos(F[j])-(V[b]-V[d])*sin(F[j]))/G6;v1=((V[b]-V[d])*(X[b]-X[d])+(U[b]-U[d])*(Y[b]-Y[d]))/G6;V[c]=V[b]-W[j]*C[i];U[c]=U[b]-W[j]*S[i];V[e]=V[d]-W[j]*(S[j]-C[k]);U[e]=U[d]+W[j]*(C[j]+S[k]);G4=A[b]-A[d]+W[j]*W[j]*(X[b]-X[d])+2*W[j]*v1*sin(F[j]);G5=B[b]-B[d]+W[j]*W[j]*(Y[b]-Y[d])-2*W[j]*v1*cos(F[j]);E[j]=(G5*cos(F[j])-G4*sin(F[j]))/G6;a1=(G4*(X[b]-X[d])+G5*(Y[b]-Y[d]))/G6;Ar=a1;Ak=2*W[j]*v1;A[e]=A[d]-E[j]*(S[j]-C[k])-W[j]*W[j]*(C[j]+S[k]);B[e]=B[d]+E[j]*(C[j]+S[k])-W[j]*W[j]*(S[j]-C[k]);Res[0]=s1;Res[1]=v1;Res[2]=a1;return(1);}intmrrp(inti,intj,intb,intc,intr,intm){doubleB0,C0,Z1,S1,X1,Y1,F1;/*定义中间变量*/doubleQ1,Q2,Q3,Q4,Q5,A1,V1;B0=2*(X[r]-X[b])*cos(F[j])+2*(Y[r]-Y[b])*sin(F[j]);S[j]=L[j]*sin(F[j]);C[j]=L[j]*cos(F[j]);C0=pow((X[r]-X[b]),2)+pow((Y[r]-Y[b]),2)+pow(L[j],2)-pow(L[i],2)-2*(X[r]-X[b])*S[j]+2*(Y[r]-Y[b])*C[j];if(B0*B0-4*C0<0)/*机构无法装配*/{return(0);/*计算失败，返回0*/}Z1=sqrt(B0*B0-4*C0);S1=(-B0+m*Z1)/2;X[c]=X[r]+S1*cos(F[j])-S[j];/*计算C点坐标*/Y[c]=Y[r]+S1*sin(F[j])+C[j];X1=X[c]-X[b];Y1=Y[c]-Y[b];F1=Angle(X1,Y1);F[i]=F1;S[i]=L[i]*sin(F[i]);C[i]=L[i]*cos(F[i]);Q1=V[r]-V[b]-W[j]*(S1*sin(F[j])+C[j]);Q2=U[r]-U[b]+W[j]*(S1*cos(F[j])-S[j]);Q3=S[i]*sin(F[j])+C[i]*cos(F[j]);W[i]=(-Q1*sin(F[j])+Q2*cos(F[j]))/Q3;V1=-(Q1*C[i]+Q2*S[i])/Q3;V[c]=V[b]-W[i]*S[i];/*计算C点速度*/U[c]=U[b]+W[i]*C[i];Q4=A[r]-A[b]+C[i]*pow(W[i],2)-E[j]*(S1*sin(F[j])+C[j])-pow(W[j],2)*(S1*cos(F[j])-S[j])-2*W[j]*V1*sin(F[j]);Q5=B[r]-B[b]+S[i]*pow(W[i],2)+E[j]*(S1*cos(F[j])-S[j])-pow(W[j],2)*(S1*sin(F[j])+C[j])+2*W[j]*V1*cos(F[j]);A1=(-Q4*C[i]-Q5*S[i])/Q3;E[i]=(-Q4*sin(F[j])+Q5*cos(F[j]))/Q3;A[c]=A[b]-E[i]*S[i]-C[i]*pow(W[i],2);/*计算C点加速度*/B[c]=B[b]+E[i]*C[i]-S[i]*pow(W[i],2);return(1);} voidmain(){intii,index,iFlagea,iFlageb;doubleP1,F9,Res[3],N1,K,M,N,P,T,R;P1=PI/180;L[1]=110;L[2]=0;L[3]=810;L[4]=0;L[5]=291.6;L[6]=0;X[1]=0;Y[1]=430;N1=72;X[4]=0;Y[4]=0;X[7]=0;printf("L[1]=110;L[2]=0;L[3]=810;L[4]=0;L[5]=291.6;L[6]=0;\n");printf("F[1]=DEGX[6]mmY[6]mmV[6]m/sA[6]m/s^2\n");T=sqrt(Y[1]*Y[1]-L[1]*L[1]);P=T*L[3]/Y[1];R=(L[3]-P)/2;Y[7]=L[3]-R;W[1]=-N1*PI/30;M=L[1]/Y[1];K=asin(M);F9=0;F[1]=-PI+K;mcrank(1,1,1,2,F9);iFlagea=mrpr(2,3,4,2,3,4,5,1,Res);if(iFlagea==0)printf("Becauseofwrongdata,theCaculationfailed\n");F[6]=0;iFlageb=mrrp(5,6,5,6,7,1);N=X[6];X[1]=-N;Y[1]=-Y[7]+Y[1];X[4]=-N;Y[4]=-Y[7];Y[7]=-N;Y[7]=0;for(ii=0;ii<=12;ii++){F[1]=-PI+K+ii*(-30)*P1;F9=0;mcrank(1,1,1,2,F9);iFlagea=mrpr(2,3,4,2,3,4,5,1,Res);if(iFlagea==0)printf("Becauseofwrongdata,theCaculationfa