fprintf(fp,\ fprintf(fp,\ for(j=0;j<6;j++) fprintf(fp,\ } fclose(fp); //输出数据 printf(\ l3=l4=%9.3f x=%9.3f h1=%9.3f\\n\ printf(\平均力矩\\t.6f\\n\ printf(\盈亏功\\t.6f\\n\ printf(\电机功率\\t.6f\\n\ printf(\ } }

/*矢量法求角位移*/

void Solutionangle(double value[M],double ang1)

{ double s3,ang3,ang4,s5; value[0]=s3=sqrt(pow(lo23+l1*cos(ang1),2)+pow(l1*sin(ang1),2)); value[1]=ang3=atan(l1*sin(ang1)/(lo23+l1*cos(ang1))); value[2]=ang4=acos((-x-l3*cos(PI+ang3))/l4); value[3]=s5=l3*sin(PI+ang3)+l4*sin(ang4)-h1; }

/*角速度求解*/

void Solutionspeed(double a2[N][N],double b2[N],double value[M],double ang1)

{

double p2[N];

GaussianE(a2,b2,p2); value[4]=p2[0]; value[5]=p2[1];

20

value[6]=p2[2]; value[7]=p2[3]; }

//角加速度求解

void Solutionacceleration(double da3[N][N],double a3[N][N],double db3[N],double value[M])

{ int i,j; double bk[N]={0}; double p3[N]; for(i=0;i

/*高斯消去法解矩阵方程*/

void GaussianE(double a4[N][N],double b4[N],double p4[N])

{

21

int i,j,k;

double a4g[N][N],b4g[N],t; for(i=0;i

a4g[i][j]=a4[i][j]; for(i=0;ia4g[1][1]) { for(j=0;ja4g[3][3]) { for(j=0;j

//创建系数矩阵A

22

void FoundmatrixA(double value5[M],double a5[N][N])

{ double s3,ang3,ang4,s5; s3=value5[0];ang3=value5[1];ang4=value5[2];s5=value5[3]; a5[0][0]=cos(ang3);a5[0][1]=-s3*sin(ang3); a5[1][0]=sin(ang3);a5[1][1]=s3*cos(ang3); a5[2][1]=-l3*sin(PI+ang3);a5[2][2]=-l4*sin(ang4); a5[3][1]=l3*cos(PI+ang3);a5[3][2]=l4*cos(ang4);a5[3][3]=-1; a5[0][2]=a5[0][3]=a5[1][2]=a5[1][3]=a5[2][0]=a5[2][3]=a5[3][0]=0; }

//创建系数矩阵B

void FoundmatrixB(double value6[M],double ang1,double b6[N])

{ b6[0]=-l1*sin(ang1)*as1; b6[1]=l1*cos(ang1)*as1; b6[2]=b6[3]=0; }

//创建矩阵DA

void FoundmatrixDA(double value7[M],double da7[N][N])

{

23

double s3,ang3,ang4,s5,ds3,as3,as4,ds5;

s3=value7[0];ang3=value7[1];ang4=value7[2];s5=value7[3]; ds3=value7[4];as3=value7[5];as4=value7[6];ds5=value7[7];

da7[0][0]=-as3*sin(ang3);da7[0][1]=-ds3*sin(ang3)-s3*as3*cos(ang3); da7[1][0]=as3*cos(ang3);da7[1][1]=ds3*cos(ang3)-s3*as3*sin(ang3); da7[2][1]=-l3*as3*cos(PI+ang3);da7[2][2]=-l4*as4*cos(ang4); da7[3][1]=-l3*as3*sin(PI+ang3);da7[3][2]=-l4*as4*sin(ang4);

da7[0][2]=da7[0][3]=da7[1][2]=da7[1][3]=da7[2][0]=da7[2][3]=da7[3][0]=da7[3][3]=0;

}

//创建矩阵DB

void FoundmatrixDB(double value8[M],double ang1,double db8[N])

{ db8[0]=-l1*as1*cos(ang1); db8[1]=-l1*as1*sin(ang1); db8[2]=db8[3]=0; }

//驱动力矩、阻力矩求解

void mf(double value9[M],double ang1)

{ double s3,ang3,ang4,s5,ds3,as3,as4,ds5,dds3,aas3,aas4,dds5,td,tr,je,ang; s3=value9[0];ang3=value9[1];ang4=value9[2];s5=value9[3]; ds3=value9[4];as3=value9[5];as4=value9[6];ds5=value9[7]; dds3=value9[8];aas3=value9[9];aas4=value9[10];dds5=value9[11]; ang=ang3+asin(az/cz); tr=g3*cz*as3*cos(ang)/as1+g5*ds5/as1; if(s5>=0.05*h && s5<0.95*h && ds5<0) tr+=-q*ds5/as1; td=tr+(g3/g)*cz*as3*pow(-1,(aas3<0))*sqrt(cz*cz*aas3*aas3+cz*as3*as3*cz*as3*as3)/as1+js3*as3*aas3/as1+g5/g*ds5*dds5/as1; value9[12]=td;value9[13]=tr; je=(g3/g)*cz*cz*as3*as3/(as1*as1)+js3*as3*as3/(as1*as1)+g5/g*ds5*ds5/(as1*as1); value9[14]=je; }

24