平面连杆机构主函数代码实现
RRR子函数
double RRR(double x1,double x1d,double x1dd,double y1,double y1d,double y1dd,double x2,double x2d,double x2dd,double y2,double y2d,double y2dd,double r1,double r2,double M,double *theta1,double *theta1d,double *theta1dd,double *theta2,double *theta2d,double *theta2dd,double *x3,double *x3d,double *x3dd,double *y3,double *y3d,double *y3dd,double *v,double *gamma,double *a,double *beta)
{
double d1,d,phi,alpha,s1,v1,a1,A,B,C,D; //统一定义函数内变量
d1 = pow(x2-x1,2)+pow(y2-y1,2); //分步计算d
d = sqrt(d1);
phi = atan((y2-y1)/(x2-x1)); //计算方位角Phi
s1 = (pow(r1,2)+d-pow(r2,2))/(2*r1*d); //分步计算内角alpha1
alpha = acos(s1);
*theta1 = phi+M*alpha; //计算构件1的方位角 theta1
*x3 = x1+r1*cos(*theta1); //计算节点3的坐标
*y3 = y1+r1*sin(*theta1);
*theta2 = atan((*y3-y2)/(*x3-x2)); //计算构件2的方位角 theta2
A = (*y3-y2)*(*x3-x1)-(*y3-y1)*(*x3-x2); //简化计算
*theta1d = ((x2d-x1d)*(*x3-x2)+(y2d-y1d)*(*y3-y2))/(A); //计算theta1的一阶导数
*theta2d = ((x2d-x1d)*(*x3-x1)+(y2d-y1d)*(*y3-y1))/(A); //计算theta2的一阶导数
*x3d = x1d - *theta1d*(*y3-y1); // 计算x3的一阶导数
*y3d = y1d + *theta1d*(*x3-x1); // 计算y3的一阶导数
v1 = pow(*x3d,2)+(*y3d,2); //分步计算速度v
*v = sqrt(v1);
*gamma = atan((*y3d)/(*x3d)); //计算速度方位角gamma
B = x2dd-x1dd+pow(*theta1d,2)*(*x3-x1)-pow(*theta2d,2)*(*x3-x2); //简化计算
C = y2dd-y1dd+pow(*theta1d,2)*(*y3-y1)-pow(*theta2d,2)*(*y3-y2); //简化计算
D = (*x3-x1)*(*y3-y2)-(*x3-x2)*(*y3-y1); //简化计算
*theta1dd = ((B)*(*x3-x2)+(C)*(*y3-y2))/(D); //计算theta1的二阶导数
*theta2dd = ((B)*(*x3-x1)+(C)*(*y3-y1))/(D); //计算theta2的二阶导数
*x3dd = x1dd-pow(*theta1d,2)*(*x3-x1)-*theta1dd*(*y3-y1); // 计算x3的二阶导数
*y3dd = y1dd-pow(*theta1d,2)*(*y3-y1)+*theta1dd*(*x3-x1); // 计算y3的二阶导数
a1 = pow(*x3dd,2)+(*y3dd,2); //分步计算加速度a
*a = sqrt(a1);
*beta = atan((*y3dd)/(*x3dd)); //计算加速度方位角beta
}
RRP子函数
double RRP(double x1,double x1d,double x1dd,double y1,double y1d,double y1dd,double x2,double x2d,double x2dd,double y2,double y2d,double y2dd,double phi,double phid,double phidd,double r,double M,double *x3,double *x3d,double *x3dd,double *y3,double *y3d,double *y3dd,double *v,double *gamma,double *a,double *beta,double *theta,double *thetad,double *thetadd,double *s,double *sd,double *sdd)
{
double d,d1,v1,E,F,K,L,O,N,A,B; //统一定义函数内变量
d1 = pow(x2-x1,2)+pow(y2-y1,2); //分步计算d
d = sqrt(d1);
E = 2*((x2-x1)*cos(phi)+(y2-y1)*sin(phi)); //简化计算
F = d*d-r*r;
*s = abs(-E+M*sqrt(E*E-4*F))/2; //求滑块与参考点的距离
*x3 = x2+*s*cos(phi); //求滑块的坐标
*y3 = y2+*s*sin(phi);
*theta = atan((*y3-y1)/(*x3-x1)); //求杆的方位角
K = x2d-x1d-*s*phid*sin(phi); //简化计算
L = y2d-y1d-*s*phid*cos(phi);
A = (*y3-y1)*sin(phi)+(*x3-x1)*cos(phi);
*thetad = (L*cos(phi)-K*sin(phi))/(A); //求杆的方位角的一阶导数
*sd = (-L*(*y3-y1)-K*(*x3-x1))/(A); //求滑块与参考点的距离的一阶导数
*x3d = x1d-*thetad*r*sin(*theta); //求滑块的坐标的一阶导数
*y3d = y1d-*thetad*r*cos(*theta);
v1 = pow(*x3d,2)+pow(*y3d,2); //分步求滑块的速度
*v = sqrt(v1);
*gamma = atan((*y3d)/(*x3d)); //求速度方位角
O = (x2dd-x1dd+pow(*thetad,2)*(*x3-x1)-pow(phid,2)*(*s)*cos(phi)-2*phid*(*s)*sin(phi)-phidd*(*y3-y2)); //简化计算
N = (y2dd-y1dd+pow(*thetad,2)*(*y3-y1)-pow(phid,2)*(*s)*sin(phi)-2*phid*(*s)*cos(phi)-phidd*(*x3-x2));
B = (*y3-y1)*sin(phi)+(*x3-x1)*cos(phi);
*thetadd = (-O*sin(phi)+N*cos(phi))/(B); //求杆的方位角的二阶导数
*sdd = (-O*(*x3-x1)-N*(*y3-y1))/(B); //求滑块与参考点的距离的二阶导数
*x3dd = x1dd-pow(*thetad,2)*r*cos(*theta)-*thetadd*r*sin(*theta); //求滑块的坐标的二阶导数
*y3dd = y1dd-pow(*thetad,2)*r*sin(*theta)+*thetadd*r*cos(*theta);
*a = sqrt((*x3dd)*(*x3dd)+(*y3dd)*(*y3dd)); //求滑块加速度
*beta = atan((*y3d)/(*x3dd)); //求加速度方位角
}
曲柄子函数
double QB(double x1,double x1d,double x1dd,double y1,double y1d,double y1dd,double phi,double phid,double phidd,double *x2,double *x2d,double *x2dd,double *y2,double *y2d,double *y2dd,double *v,double *gamma,double *a,double *beta)
{
*x2 = x1+cos(phi); //求远端点的坐标
*y2 = y1+sin(phi);
*x2d = x1d-sin(phi)*phid; //求远端点坐标的一阶导数
*y2d = y1d+cos(phi)*phid;
*v = sqrt((*x2d)*(*x2d)+(*y2d)*(*y2d)); //求远端点速度
*gamma = atan((*y2d)/(*x2d)); //速度方位角
*x2dd = x1dd-cos(phi)*phid*phid-sin(phi)*phidd; //求远端点坐标的二阶导数
*y2dd = y1dd-sin(phi)*phid*phid+cos(phi)*phidd;
*a = sqrt((*x2dd)*(*x2dd)+(*y2dd)*(*y2dd)); //求远端点加速度
*beta = atan((*y2dd)/(*x2dd)); //求加速度方位角
}