RTCP完整算法

Tualar 发表于 2025-12-4 18:50:02

#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;
}

jonhsonsu 发表于 2025-12-4 19:04:32

NB，虽然我看不懂

黄帝啊 发表于 2025-12-4 19:45:45

jonhsonsu 发表于 2025-12-4 19:04
NB，虽然我看不懂

哈哈看得头疼

年轻的勇士 发表于 2025-12-4 23:33:00

NB，感谢分享

m998899 发表于 2025-12-4 23:40:58

看不懂，但感觉很nb

花开年少 发表于 2025-12-5 00:16:12

牛逼啊大佬

hegaoj3 发表于 2025-12-5 00:38:05

y谢谢分享

leilei1011 发表于 2025-12-5 07:59:16

好复杂。。

迷航* 发表于 2025-12-5 08:00:42

牛B!(破音~)，我也看不懂！

页: [1]

UG爱好者's Archiver

RTCP完整算法