Tualar 发表于 2025-12-21 14:47:13

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

//计算五轴BC宏联动X坐标
CString Calc_BC_X(double x, double b, double c)
{
CString s, s1, s2, s3, s4;
double v1, v2, v3, w1, w2, w3;
      v1 = r6(cos(b*RAD)*cos(c*RAD)-1.0);
      v2 = r6(cos(b*RAD)*sin(c*RAD));
      v3 = r6(-1.0*sin(b*RAD));
      w1 = fabs(v1);
      w2 = fabs(v2);
      w3 = fabs(v3);
      s1 = e0(fabs(v1));
      s2 = e0(fabs(v2));
      s3 = e0(fabs(v3));
      s4 = d0(fabs(x));
      if (v1 > 0.0 && v2 > 0.0 && v3 > 0.0 && x > 0.0)
      { //v1>0,v2>0,v3>0,x>0
      if (w1 == 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[#101+#102+#103+%s]", s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#102+%s*#103+%s]", s2, s3, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[%s*#101+#102+%s*#103+%s]", s1, s3, s4);
      else if (w1 != 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+%s*#102+#103+%s]", s1, s2, s4);
      else if (w1 == 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[#101+#102+%s*#103+%s]", s3, s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[#101+%s*#102+#103+%s]", s2, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#102+#103+%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#102+%s*#103+%s]", s1, s2, s3, s4);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 > 0.0 && x == 0.0)
      { //v1>0,v2>0,v3>0,x=0
      if (w1 == 1.0 && w2 == 1.0 && w3 == 1.0)
          s = "X[#101+#102+#103]";
      else if (w1 == 1.0 && w2 != 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#102+%s*#103]", s2, s3);
      else if (w1 != 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[%s*#101+#102+%s*#103]", s1, s3);
      else if (w1 != 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+%s*#102+#103]", s1, s2);
      else if (w1 == 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[#101+#102+%s*#103]", s3);
      else if (w1 == 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[#101+%s*#102+#103]", s2);
      else if (w1 != 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#102+#103]", s1);
      else s.Format("X[%s*#101+%s*#102+%s*#103]", s1, s2, s3);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 > 0.0 && x < 0.0)
      { //v1>0,v2>0,v3>0,x<0
      if (w1 == 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[#101+#102+#103-%s]", s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#102+%s*#103-%s]", s2, s3, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[%s*#101+#102+%s*#103-%s]", s1, s3, s4);
      else if (w1 != 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+%s*#102+#103-%s]", s1, s2, s4);
      else if (w1 == 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[#101+#102+%s*#103-%s]", s3, s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[#101+%s*#102+#103-%s]", s2, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#102+#103-%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#102+%s*#103-%s]", s1, s2, s3, s4);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 == 0.0 && x > 0.0)
      { //v1>0,v2>0,v3=0,x>0
      if (w1 == 1.0 && w2 == 1.0)
          s.Format("X[#101+#102+%s]", s4);
      else if (w1 == 1.0 && w2 != 1.0)
          s.Format("X[#101+%s*#102+%s]", s2, s4);
      else if (w1 != 1.0 && w2 == 1.0)
          s.Format("X[%s*#101+#102+%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#102+%s]", s1, s2, s4);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 == 0.0 && x == 0.0)
      { //v1>0,v2>0,v3=0,x=0
      if (w1 == 1.0 && w2 == 1.0)s = "X[#101+#102]";
      else if (w1 == 1.0 && w2 != 1.0)
          s.Format("X[#101+%s*#102]", s2);
      else if (w1 != 1.0 && w2 == 1.0)
          s.Format("X[%s*#101+#102]", s1);
      else s.Format("X[%s*#101+%s*#102]", s1, s2);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 == 0.0 && x < 0.0)
      { //v1>0,v2>0,v3=0,x<0
      if (w1 == 1.0 && w2 == 1.0)
          s.Format("X[#101+#102-%s]", s4);
      else if (w1 == 1.0 && w2 != 1.0)
          s.Format("X[#101+%s*#102-%s]", s2, s4);
      else if (w1 != 1.0 && w2 == 1.0)
          s.Format("X[%s*#101+#102-%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#102-%s]", s1, s2, s4);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 < 0.0 && x > 0.0)
      { //v1>0,v2>0,v3<0,x>0
      if (w1 == 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[#101+#102-#103+%s]", s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#102-%s*#103+%s]", s2, s3, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[%s*#101+#102-%s*#103+%s]", s1, s3, s4);
      else if (w1 != 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+%s*#102-#103+%s]", s1, s2, s4);
      else if (w1 == 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[#101+#102-%s*#103+%s]", s3, s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[#101+%s*#102-#103+%s]", s2, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#102-#103+%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#102-%s*#103+%s]", s1, s2, s3, s4);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 < 0.0 && x == 0.0)
      { //v1>0,v2>0,v3<0,x=0
      if (w1 == 1.0 && w2 == 1.0 && w3 == 1.0)
          s = "X[#101+#102-#103]";
      else if (w1 == 1.0 && w2 != 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#102-%s*#103]", s2, s3);
      else if (w1 != 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[%s*#101+#102-%s*#103]", s1, s3);
      else if (w1 != 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+%s*#102-#103]", s1, s2);
      else if (w1 == 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[#101+#102-%s*#103]", s3);
      else if (w1 == 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[#101+%s*#102-#103]", s2);
      else if (w1 != 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#102-#103]", s1);
      else s.Format("X[%s*#101+%s*#102-%s*#103]", s1, s2, s3);
      }
      else if (v1 > 0.0 && v2 > 0.0 && v3 < 0.0 && x < 0.0)
      { //v1>0,v2>0,v3<0,x<0
      if (w1 == 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[#101+#102-#103-%s]", s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#102-%s*#103-%s]", s2, s3, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[%s*#101+#102-%s*#103-%s]", s1, s3, s4);
      else if (w1 != 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+%s*#102-#103-%s]", s1, s2, s4);
      else if (w1 == 1.0 && w2 == 1.0 && w3 != 1.0)
          s.Format("X[#101+#102-%s*#103-%s]", s3, s4);
      else if (w1 == 1.0 && w2 != 1.0 && w3 == 1.0)
          s.Format("X[#101+%s*#102-#103-%s]", s2, s4);
      else if (w1 != 1.0 && w2 == 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#102-#103-%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#102-%s*#103-%s]", s1, s2, s3, s4);
      }
      else if (v1 > 0.0 && v2 == 0.0 && v3 > 0.0 && x > 0.0)
      { //v1>0,v2=0,v3>0,x>0
      if (w1 == 1.0 && w3 == 1.0)
          s.Format("X[#101+#103+%s]", s4);
      else if (w1 == 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#103+%s]", s3, s4);
      else if (w1 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#103+%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#103+%s]", s1, s3, s4);
      }
      else if (v1 > 0.0 && v2 == 0.0 && v3 > 0.0 && x == 0.0)
      { //v1>0,v2=0,v3>0,x=0
      if (w1 == 1.0 && w3 == 1.0)s = "X[#101+#103]";
      else if (w1 == 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#103]", s3);
      else if (w1 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#103]", s1);
      else s.Format("X[%s*#101+%s*#103]", s1, s3);
      }
      else if (v1 > 0.0 && v2 == 0.0 && v3 > 0.0 && x < 0.0)
      { //v1>0,v2=0,v3>0,x<0
      if (w1 == 1.0 && w3 == 1.0)
          s.Format("X[#101+#103-%s]", s4);
      else if (w1 == 1.0 && w3 != 1.0)
          s.Format("X[#101+%s*#103-%s]", s3, s4);
      else if (w1 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101+#103-%s]", s1, s4);
      else s.Format("X[%s*#101+%s*#103-%s]", s1, s3, s4);
      }
      else if (v1 > 0.0 && v2 == 0.0 && v3 == 0.0 && x > 0.0)
      { //v1>0,v2=0,v3=0,x>0
      if (w1 == 1.0)s.Format("X[#101+%s]", s4);
      else s.Format("X[%s*#101+%s]", s1, s4);
      }
      else if (v1 > 0.0 && v2 == 0.0 && v3 == 0.0 && x == 0.0)
      { //v1>0,v2=0,v3=0,x=0
      if (w1 == 1.0)s = "X[#101]";
      else s.Format("X[%s*#101]", s1);
      }
      else if (v1 > 0.0 && v2 == 0.0 && v3 == 0.0 && x < 0.0)
      { //v1>0,v2=0,v3=0,x<0
      if (w1 == 1.0)s.Format("X[#101-%s]", s4);
      else s.Format("X[%s*#101-%s]", s1, s4);
      }
      else if (v1 > 0.0 && v2 == 0.0 && v3 < 0.0 && x > 0.0)
      { //v1>0,v2=0,v3<0,x>0
      if (w1 == 1.0 && w3 == 1.0)
          s.Format("X[#101-#103+%s]", s4);
      else if (w1 == 1.0 && w3 != 1.0)
          s.Format("X[#101-%s*#103+%s]", s3, s4);
      else if (w1 != 1.0 && w3 == 1.0)
          s.Format("X[%s*#101-#103+%s]", s1, s4);
      else s.Format("X[%s*#101-%s*#103+%s]", s1, s3, s4);
      }
未完待续.............

都错了 发表于 2025-12-21 15:56:06

:handshake:handshake

gosh 发表于 2025-12-21 16:38:02

楼主威武，下载了，试试看是否神奇

gsphzc 发表于 2025-12-21 17:58:57

谢谢分享~!~~~

lj564 发表于 2025-12-21 21:39:26

也可以做曲面大圆弧插值运算或表面恒进给速度控制运算吗？

时间都去哪了？ 发表于 2025-12-21 21:42:08

谢谢分享

blackluna 发表于 2025-12-22 09:32:02

谢谢分享

hegaoj3 发表于 2026-1-2 23:12:49

谢谢分享
页: [1]
查看完整版本: 四轴五轴宏联动算法(三)