五轴摇篮AC/BC刀尖跟随RTCP算法
//支持3+2定轴G54变化的计算//支持五轴联动刀尖点变化的跟踪
#define rad ((double)0.01745329252)/*角度转换为弧度π/180*/
#define AC (UINT)0//AC摇篮机床
#define BC (UINT)1//BC摇篮机床
typedef struct
{
double x, y, z, axle4, axle5; //基于工件坐标系的NC指令数据G1 X,Y,Z,A/B,C。axle4=A/B轴角度,axle5=C轴角度。
double x5, y5, z5, x4, y4, z4; //用来接收旋转轴转动后刀尖点位置变化的计算结果, 最终RTCP计算结果保存在x4,y4,z4里面。
double rcpx, rcpy, rcpz; //第四轴A/B摇篮中心线和第五轴转台中心线的交点在机床坐标系中的位置。通过标定找到回转中心。
double coordx, coordy, coordz; //分中/工件坐标系G54数据。
}rtcp_t;
//RTCP计算函数：
//参数p: 指向rtcp_t结构的指针。
//参数type: 机床结构力学，取AC或者BC。
//返回值: TRUE表示计算成功; FALSE表示计算失败。
BOOL RTCP_Algorithm(rtcp_t* p, UINT type)
{
if ( !p || ( type != AC && type != BC)) return FALSE;
//先计算五轴转台:
double x, y, z, X, Y, Z;
double sin4, cos4, sin5, cos5;
sin5 = sin(p->axle5*rad);
cos5 = cos(p->axle5*rad);
x = p->rcpx + p->coordx + p->x;
y = p->rcpy + p->coordy + p->y;
z = p->rcpz + p->coordz + p->z;
X = x*sin5 + y*cos5;
Y = x*cos5 - y*sin5;
Z = z;
p->x5 = X - p->rcpx - p->coordx;
p->y5 = Y - p->rcpy - p->coordy;
p->z5 = Z - p->rcpz - p->coordz;
//后计算四轴摇篮:
x = p->rcpx + p->coordx + p->x5;
y = p->rcpy + p->coordy + p->y5;
z = p->rcpz + p->coordz + p->z5;
sin4 = sin(-(p->axle4)*rad);
cos4 = cos(-(p->axle4)*rad);
switch (type)
{
case AC:
X = x;
Y = y*cos4 - z*sin4;
Z = y*sin4 + z*cos4;
break;
case BC:
X = x*cos4 + z*sin4;
Y = y;
Z = -x*sin4 + z*cos4;
break;
}
//计算结果保存在X4,Y4,Z4变量中：
p->x4 = X - p->rcpx - p->coordx;
p->y4 = Y - p->rcpy - p->coordy;
p->z4 = Z - p->rcpz - p->coordz;
return TRUE;
}
感谢分享~ 然后呢:lol 谢谢分享 谢谢分享 谢谢分享 感谢分享，学习一下，！
页:
[1]