牛头刨床c语言编程,牛头刨床(C语言源程序)(工科机械).doc

牛头刨床(C语言源程序)

#include

#include

#define PI 3.14159

#define E1 1e-3

#define E0 1e-9

#define m 4

#define N0 500 //控制牛顿迭代次数

double F(double x[m],double t2,int n) //角位移代数式组

{

double p[m];

p[0]=3.0*cos(x[2])-x[3];

p[1]=3.0*sin(x[2])-1.2-x[0];

p[2]=x[1]*cos(x[2])-0.5*cos(t2/180*PI);

p[3]=x[1]*sin(x[2])-0.5*sin(t2/180*PI)-x[0];

return p[n];

}

double A(double x[m],int i,int j) //建立雅可比矩阵

{

double p[m][m];

p[0][0]=0;p[0][1]=0;p[0][2]=-3.0*sin(x[2]);p[0][3]=-1;

p[1][0]=-1;p[1][1]=0;p[1][2]=3.0*cos(x[2]);p[1][3]=0;

p[2][0]=0;p[2][1]=cos(x[2]);p[2][2]=-x[1]*sin(x[2]);p[2][3]=0;

p[3][0]=-1;p[3][1]=sin(x[2]);p[3][2]=x[1]*cos(x[2]);p[3][3]=0;

return p[i][j];

}

double G(double a[m][m],int n,double b[m]) //用高斯消去法求解线性方程组(系数矩阵为方阵)

{

int i,j,k,i1,j1;

double temp;

int row_maxmod;

double element_maxmod;

for(j=0;j

{

element_maxmod=a[j][j];

row_maxmod=j;

for(i=j+1;i

if(fabs(element_maxmod)

{

element_maxmod=a[i][j];

row_maxmod=i;

}

if(fabs(element_maxmod)

{

printf("系数行列式为零，线性方程组无解!\n");

return NULL;

}

if(row_maxmod!=j)

{

for(k=j;k

{

temp=a[j][k];

a[j][k]=a[row_maxmod][k];

a[row_maxmod][k]=temp;

}

temp=b[j];

b[j]=b[row_maxmod];

b[row_maxmod]=temp;

}

for(k=j;k

a[j][k]/=element_maxmod;

b[j]/=element_maxmod;

for(i1=j+1;i1<4;i1++)

{

temp=a[i1][j];

for(j1=j;j1<4;j1++)

a[i1][j1]-=a[j][j1]*temp;

b[i1]-=b[j]*temp;

}

}

for(i=m-2;i>=0;i--) //逆序求线性方程组的解

for(j=3;j>i;j--)

b[i]-=a[i][j]*b[j];

return b[n];

}

double C(double x[m],double v[m], double t2,int n) //建立角加速度方程常数项

{

double p[m];

p[0]=3.0*v[2]*v[2]*cos(x[2]);

p[1]=3.0*v[2]*v[2]*sin(x[2]);

p[2]=-0.5*cos(t2*PI/180)+2*(v[1])*(v[2])*sin(x[2])+(x[1])*(v[2])*(v[2])*cos(x[2]);

p[3]=-0.5*sin(t2*PI/180)-2*(v[1])*(v[2])*cos(x[2])+(x[1])*(v[2])*(v[2])*sin(x[2]);

return p[n];

}

int main()

{

int