RTCP完整算法

Tualar

#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

NB，虽然我看不懂

黄帝啊

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

哈哈 看得头疼