|
|
#define AC (UINT)1
#define BC (UINT)2
#define A4 (UINT)3
#define B4 (UINT)4
typedef struct
{
double x, y, z, a, b, c; //输入x,y,z,a,b,c
double x5, y5, z5; //保存第五轴x,y,z,a,b,c
double X, Y, Z, A, B, C; //保存计算结果x,y,z,a,b,c
double g55x, g55y, g55z; //保存旋转轴中心线交点的机床坐标位置G55
double g56x, g56y, g56z; //保存用分中棒抓取的工件坐标位置G56
double tlen; //刀具长度
}rtcp_t;
typedef struct
{
double a, b, c; //输入a,b,c
double X, Y, Z, A, B, C; //保存G54最终计算结果
double g55x, g55y, g55z; //保存旋转轴中心线交点的机床坐标位置G55
double g56x, g56y, g56z; //保存用分中棒抓取的工件坐标位置G56
}g54_t;
typedef struct
{
double a, b, c; //旋转轴角度配置
}offset_t;
3+2定轴算法：
BOOL g54_fixed_axis_algorithm(g54_t* p, offset_t* pOffset, UINT type)
{
if (!p || !pOffset || type < AC || type > B4) return FALSE;
double x, y, z, x4, y4, z4, x5, y5, z5;
double sin4, cos4, sin5, cos5;
switch (type)
{
case AC:
sin4 = -1.0*sin(p->a*rad);
cos4 = cos(p->a*rad);
sin5 = -1.0*sin(p->c*rad);
cos5 = cos(p->c*rad);
break;
case A4:
sin4 = sin(-(p->a)*rad);
cos4 = cos(p->a*rad);
sin5 = 0.0;
cos5 = 1.0;
break;
case BC:
sin4 = -1.0*sin(p->b*rad);
cos4 = cos(p->b*rad);
sin5 = -1.0*sin(p->c*rad);
cos5 = cos(p->c*rad);
case B4:
sin4 = sin(-(p->b)*rad);
cos4 = cos(p->b*rad);
sin5 = 0.0;
cos5 = 1.0;
break;
}
x5 = p->g56x - p->g55x;
y5 = p->g56y - p->g55y;
z5 = p->g56z - p->g55z;
//5 axis calclate:
x4 = x5*cos5 - y5*sin5;
y4 = x5*sin5 + y5*cos5;
z4 = z5;
//4 axis calclate:
switch (type)
{
case AC:
case A4:
x = x4;
y = y4*cos4 - z4*sin4;
z = y4*sin4 + z4*cos4;
break;
case BC:
case B4:
x = x4*cos4 + z4*sin4;
y = y4;
z = z4*cos4 - x4*sin4;
break;
}
p->X = x + p->g55x;
p->Y = y + p->g55y;
p->Z = z + p->g55z;
p->A = p->a + pOffset->a;
p->B = p->b + pOffset->b;
p->C = p->c + pOffset->c;
return TRUE;
}
五轴联动刀尖跟随RTCP算法：
BOOL g53_rtcp_algorithm(rtcp_t* p, offset_t* pOffset, UINT type)
{
if (!p || !pOffset || type < AC || type > B4) return FALSE;
double x, y, z, x4, y4, z4, x5, y5, z5;
double sin4, cos4, sin5, cos5;
switch (type)
{
case AC:
sin4 = -1.0*sin(p->a*rad);
cos4 = cos(p->a*rad);
sin5 = -1.0*sin(p->c*rad);
cos5 = cos(p->c*rad);
break;
case A4:
sin4 = -1.0*sin(p->a*rad);
cos4 = cos(p->a*rad);
sin5 = 0.0;
cos5 = 1.0;
break;
case BC:
sin4 = -1.0*sin(p->b*rad);
cos4 = cos(p->b*rad);
sin5 = -1.0*sin(p->c*rad);
cos5 = cos(p->c*rad);
case B4:
sin4 = -1.0*sin(p->b*rad);
cos4 = cos(p->b*rad);
sin5 = 0.0;
cos5 = 1.0;
break;
}
x5 = p->g56x - p->g55x + p->x;
y5 = p->g56y - p->g55y + p->y;
z5 = p->g56z - p->g55z + p->z;
//5 axis calclate:
x4 = x5*cos5 - y5*sin5;
y4 = x5*sin5 + y5*cos5;
z4 = z5;
p->x5 = x4;
p->y5 = y4;
p->z5 = z4 + p->tlen;
//4 axis calclate:
switch (type)
{
case AC:
case A4:
x = x4;
y = y4*cos4 - z4*sin4;
z = y4*sin4 + z4*cos4;
break;
case BC:
case B4:
x = x4*cos4 + z4*sin4;
y = y4;
z = z4*cos4 - x4*sin4;
break;
}
p->X = x; //机械坐标X
p->Y = y; //机械坐标Y
p->Z = z + p->tlen; //机械坐标Z偏移一个刀长
p->A = p->a + pOffset->a; //机械坐标a偏移一个偏置a
p->B = p->b + pOffset->b; //机械坐标b偏移一个偏置b
p->C = p->c + pOffset->c; //机械坐标c偏移一个偏置c
return TRUE;
}
|
|
发表于 2025-12-4 18:50:02
