Tualar 发表于 2025-12-20 06:06:25

四轴五轴宏联动算法(一)

// 适用于发那科，三菱，华中，广数等ISO系统

//宏联动原理：
//用宏运算实现加工过程中RTCP刀尖跟随实时计算,联动加工

//将一个实数截断为6位精度

double r6(double data)
{
CString s;
s.Format("%.6f", data);
s.Trim();
return atof(s);
}
//保留小数点后4位,并去除冗余0
CString d0(double data)
{
CString s, z1, z2, z3, z4;
if (fabs(data) != 0.0)
{
    s.Format("%.4f", data);
    int pos = s.Find('.');
    if (pos != -1)
    {
      z1 = s.Mid(pos + 4, 1);
      z2 = s.Mid(pos + 3, 2);
      z3 = s.Mid(pos + 2, 3);
      z4 = s.Mid(pos + 1, 4);
      if (!z4.Compare("0000"))s.Delete(pos + 1, 4);
      else if (!z3.Compare("000"))s.Delete(pos + 2, 3);
      else if (!z2.Compare("00"))s.Delete(pos + 3, 2);
      else if (!z1.Compare("0"))s.Delete(pos + 4, 1);
    }
} else s = "0.";
return s;
}
// 保留小数点后6位,并去除冗余0
CString e0(double data)
{
CString s, z1, z2, z3, z4, z5 ,z6;
if (fabs(data) != 0.0)
{
    s.Format("%.6f", data);
    s.Trim();
    int pos = s.Find('.');
    if (pos != -1)
    {
      z1 = s.Mid(pos + 6, 1);
      z2 = s.Mid(pos + 5, 2);
      z3 = s.Mid(pos + 4, 3);
      z4 = s.Mid(pos + 3, 4);
      z5 = s.Mid(pos + 2, 5);
      z6 = s.Mid(pos + 1, 6);
      if (!z6.Compare("000000"))s.Delete(pos + 1, 6);
      else if (!z5.Compare("00000"))s.Delete(pos + 2, 5);
      else if (!z4.Compare("0000"))s.Delete(pos + 3, 4);
      else if (!z3.Compare("000"))s.Delete(pos + 4, 3);
      else if (!z2.Compare("00"))s.Delete(pos + 5, 2);
      else if (!z1.Compare("0"))s.Delete(pos + 6, 1);
    }
} else s = "0.";
return s;
}
//范例:
/*
1. UG后处理的程序:
T1 M6
G0 G90 G54 X-5.0133 Y-11.6639 A30. C90. S5000 M3
G43 Z29.8282 H1 M8
G1 Z.953 F10000.
X-5.0089 Y-11.8602 F2000.
X-4.9968 Y-12.0562
X-4.977 Y-12.2515
............
X0. Y-16.6506
C90.3516
C90.7031
C91.0547
..........
X5.0089 Y-11.8602
X5.0133 Y-11.6639
Z29.8282 F10000.
G91 G28 Z0.
G91 G28 X0. Y0. M9
G91 G28 A0. C0. M5
M30
%
2. 将UG后处理的NC程序转换成宏联动的NC程序:
T1 M6
#101=#5261-#5241
#102=#5262-#5242
#103=#5263-#5243
(将分中坐标系G56在A0,C0处机床坐标写入G54里面)
G90 G10 L2 P1 X110.837 Y59.357 Z-220.
G0 G90 G54 X[-#101+#102-5.0133] Y[-#102-0.866025*#101+0.5*#103-11.6639] A30. C90. S5000 M3
G43 Z H1 M8
G1 Z F10000.
X[-#101+#102-5.0089] Y[-#102-0.866025*#101+0.5*#103-11.8602] F2000.
X[-#101+#102-4.9968] Y[-#102-0.866025*#101+0.5*#103-12.0562]
X[-#101+#102-4.977] Y[-#102-0.866025*#101+0.5*#103-12.2515]
............
X[-#101+#102] Y[-#102-0.866025*#101+0.5*#103-16.6506]
X[-1.006137*#101+0.999981*#102]Y[-1.005314*#102-0.866009*#101+0.5*#103-16.6506]Z C90.3516
X[-1.012271*#101+0.999925*#102]Y[-1.010627*#102-0.86596*#101+0.5*#103-16.6506]Z C90.7031
X[-1.018407*#101+0.999831*#102]Y[-1.015941*#102-0.865879*#101+0.5*#103-16.6506]Z C91.0547
.........
X[-#101+#102+4.9968] Y[-#102-0.866025*#101+0.5*#103-12.0562]
X[-#101+#102+5.0089] Y[-#102-0.866025*#101+0.5*#103-11.8602]
X[-#101+#102+5.0133] Y[-#102-0.866025*#101+0.5*#103-11.6639]
Z F10000.
G91 G28 Z0.
G91 G28 X0. Y0. M9
G91 G28 A0. C0. M5
M30
%
*/

//算法的C/C++语言描述:
#define RAD   ((double)0.01745329252)/*角度转弧度RAD=π/180.0=4.0*atan(1.0)/180.0*/
typedef enum { AC, BC, A4, B4 }mach_t; //机床类型：五轴AC,五轴BC,四轴A,四轴B
typedef struct { double x, y, z; }xyz_t;
typedef struct { double x, y; }xy_t;
typedef struct { double x, y, z, a, b, c; }pos_t;
typedef struct
{
pos_t pos;//NC程序行中的X,Y,Z,A,B,C数据
xyz_t rcp;//旋转中心点G55,五轴机床是四轴五轴中心线交点,四轴机床是四轴中心线上的任意一点
xyz_t mcs;//工件/分中坐标系G56
xy_tdev;//机床装配偏差,主轴端面中心点与旋转中心点的XY偏差,通常设置为0,忽略不计
}rtcp_t;

//计算G54,相当于G68.2,定轴加工
pos_t Calc_G54(rtcp_t data)
{
pos_t pos;
double e, x, y, z, u, v, w;
double sin4, cos4, sin5, cos5;
u = data.mcs.x - data.rcp.x;
v = data.mcs.y - data.rcp.y;
w = data.mcs.z - data.rcp.z;
switch (machine)
{
case AC:
    sin4 = sin(data.pos.a*RAD);
    cos4 = cos(data.pos.a*RAD);
    sin5 = sin(data.pos.c*RAD);
    cos5 = cos(data.pos.c*RAD);
    e = v*cos5 - u*sin5;
    x = u*cos5 + v*sin5;
    y = e*cos4 + w*sin4;
    z = w*cos4 - e*sin4;
    pos.x = x + data.rcp.x - data.dev.x;
    pos.y = y + data.rcp.y - data.dev.y;
    pos.z = z + data.rcp.z;
    pos.a = data.pos.a;
    pos.c = data.pos.c;
    break;
case BC:
    sin4 = sin(data.pos.b*RAD);
    cos4 = cos(data.pos.b*RAD);
    sin5 = sin(data.pos.c*RAD);
    cos5 = cos(data.pos.c*RAD);
    e = u*cos5 + v*sin5;
    x = e*cos4 - w*sin4;
    y = v*cos5 - u*sin5;
    z = w*cos4 + e*sin4;
    pos.x = x + data.rcp.x - data.dev.x;
    pos.y = y + data.rcp.y - data.dev.y;
    pos.z = z + data.rcp.z;
    pos.b = data.pos.b;
    pos.c = data.pos.c;
    break;
case A4:
    sin4 = sin(data.pos.a*RAD);
    cos4 = cos(data.pos.a*RAD);
    x = data.mcs.x;
    y = v*cos4 + w*sin4;;
    z = w*cos4 - v*sin4;
    pos.x = x - data.dev.x;
    pos.y = y + data.rcp.y - data.dev.y;
    pos.z = z + data.rcp.z;
    pos.a = data.pos.a;
    break;
case B4:
    sin4 = sin(data.pos.b*RAD);
    cos4 = cos(data.pos.b*RAD);
    x = u*cos4 - w*sin4;
    y = data.mcs.y;
    z = w*cos4 + u*sin4;
    pos.x = x + data.rcp.x - data.dev.x;
    pos.y = y - data.dev.y;
    pos.z = z + data.rcp.z;
    pos.b = data.pos.b;
    break;
}
return pos;
}

//计算RTCP,刀尖跟随,相当于G43.4,联动加工
pos_t CMy_5x_RTCP_Dlg::Calc_Rtcp(rtcp_t data)
{
pos_t rtcp, g54 = Calc_G54(data);
rtcp.x = g54.x - data.mcs.x + data.pos.x;
rtcp.y = g54.y - data.mcs.y + data.pos.y;
rtcp.z = g54.z - data.mcs.z + data.pos.z;
switch (machine)
{
    case AC:
      rtcp.a = data.pos.a;
      rtcp.b = 0.0;
      rtcp.c = data.pos.c;
      break;
    case BC:
      rtcp.a = 0.0;
      rtcp.b = data.pos.b;
      rtcp.c = data.pos.c;
      break;
    case A4:
      rtcp.a = data.pos.a;
      rtcp.b = rtcp.c = 0.0;
      break;
    case B4:
      rtcp.b = data.pos.b;
      rtcp.a = rtcp.c = 0.0;
      break;
}
return rtcp;
}




时间都去哪了？ 发表于 2025-12-20 14:09:35

谢谢分享，虽然看不懂

Addictive 发表于 2025-12-20 17:00:25

谢谢分享

Tualar 发表于 2025-12-21 14:21:49

本帖最后由 Tualar 于 2025-12-21 22:54 编辑

时间都去哪了？ 发表于 2025-12-20 14:09
谢谢分享，虽然看不懂
未完待续.............

blackluna 发表于 2025-12-22 09:30:38

谢谢分享

renxinying 发表于 2025-12-26 10:25:44

TCL语言大佬

HANFZ88 发表于 2025-12-27 09:41:51

感谢分享，虽然目前还看不懂

hegaoj3 发表于 2026-1-2 23:11:07

谢谢分享

fbph2003 发表于 2026-1-10 22:05:35

学习了，牛:victory:
页: [1]
查看完整版本: 四轴五轴宏联动算法(一)