codesys凸轮中编写自定义圆弧插补以及直线插补程序

显示全部楼层 · 2025-1-2 08:16:03

一，前言

之前在使用凸轮时候，只是简单的计算出所需要的路径，并没有开发一个接口给到操机的人员去做一些自定义的编程。

前段时间设计同时编程了一台也是需要点胶的设备，选型的时候看上了基恩士KV8000的点参数功能，接口是开放给PLC编程人员，可以定义插补运动的路径；之后也发现点参数的编辑KV-XH16EC运动扩展也开放了变量设置，所以就在GOT做了可提供给操机人员自定义编程的接口。

回到之前基于codesys的凸轮控制中，一直在思考是否也可以做到自定义编程，最近刚好设备需要修改点胶路径，因此就动起了键盘鼠标，开始了编程。

二，圆弧插补圆的路径

在之前也有用过其他品牌的点胶控制器，比如日本的武藏，成都的乐创，以及橙黑手柄不知名的点胶手柄。圆弧的设定一般有三轴模式，第一是已知起点，终点，圆心；第二是已知起点，终点，半径；第三是已知起点，终点，中间点。因此在做圆弧插补的过程中需要有这三种设定。但如何从计算中输入已知量，就可以得到我们想知道的结果呢？我们需要的结果是起点，圆心，终点，半径，以及弧度。

1.已知起点，终点，半径的思路

已知圆的半径R，以及圆上的两点起点A(XA,YA)，终点B(XB,YB)求圆的圆心：

首先需要判定起点A，终点B两点的距离是否大于半径，若是大于半径，则代表两点并不在该圆上；第二判定起点和终点两点间的距离是否等于0，这步不判断也可以，证明起点和终点一样，画一个圆。

进入解题：

首先，将整个坐标系以A为坐标原点，则A(0,0)，B(XB-XA,YB-YA)；

第二，计算A，B两点之间的长度L；

第三，将以A为坐标原点的坐标系进行逆平次变换，将B(XB-XA,YB-YA)变换成(L,0)；既是以A为原点逆时针旋转第四中的θ角，让A与B在同一X坐标轴上；

第四，平次变换是x1=xcosθ-ysinθ，y1=xsinθ+ycosθ：

cosθ=(XB-XA)/L，sinθ=(YB-YA)/L；

第五，圆心在逆平次坐标的坐标是(L/2,±√{R*R-[L/2]*[L/2]})；

第六，经过平次坐标后坐标是：

([L/2]*cosθ-(±√{R*R-[L/2]*[L/2]})*sinθ,[L/2]*sinθ+(±√{R*R-[L/2]*[L/2]})*cosθ)；

第七，从以A为坐标原点变换到实际坐标后圆心的坐标是：

([L/2]*cosθ-(±√{R*R-[L/2]*[L/2]})*sinθ+XA,[L/2]*sinθ+(±√{R*R-[L/2]*[L/2]})*cosθ+YA)；

按照以上的思路，可以求解出圆心的坐标，同样，半径，以及弧长都可以求解。在求解过程需要考虑顺逆时针，以及劣弧优弧的分类。

2.已知起点，终点，中间点

已知圆上的三点起点(x1,y1)，中间点(x2,y2)，终点(x3,y3)，求圆心：

首先需要判定三点是否在同一条直线上，需要判定两组两点间的斜率，若是斜率相等，则无法形成一个圆。

进入解题：

首先，三点到圆心的距离是一样的，因此有：

(x-x1)^2+(y-y1)^2=(x-x2)^2+(y-y2)^2，

(x-x1)^2+(y-y1)^2=(x-x3)^2+(y-y3)^2；

第二，方程化简后：

2(x1-x2)x+2(y1-y2)y=x1^2-x2^2+y1^2-y2^2，

2(x1-x3)x+2(y1-y3)y=x1^2-x3^2+y1^2-y3^2；

第三，令a=x1-x2，b=y1-y2，c=x1-x3，d=y1-y3，e=x1^2-x2^2+y1^2-y2^2，f=x1^2-x3^2+y1^2-y3^2；

第四，由此可以得出x=1/2[(bf-de)/(bc-ad)]，y=1/2[(ce-af)/(bc-ad)]；

引用的链接：圆上三点坐标计算圆心坐标 - wesson2019 - 博客园(https://www.cnblogs.com/wesson2019-blog/p/17459969.html)

按照公式求解后，需要考虑bc-ad，以及bc-ad是否等于0的情况，可以根据是否有两点的坐标相同作为判定。

在程序的结尾需要判定圆弧是否是半圆？

3.已知起点，终点，圆心

如果前面两种能够求解，那么第三种基本上没有难度。

4.已知起点，终点，圆心，半径，弧度——求插补座标

构建三轴圆弧插补的思路

1，需要得知三轴圆弧线的长度

单条圆弧运动可以想象是圆柱体上一条圆弧线LXYZ，高度是Z轴方向的高度差H，长度是起点(X1,Y1)，终点(X2,Y2)，圆柱体在Z方向的投影上的一个圆的一段圆弧LXY，需要知道圆弧LXY的长度L，就可以算出圆柱体上LXYZ的长度。

2，需要知道起点(X1,Y1)，终点(X2,Y2)在圆的周长上的长度以及求解思路

引用的文章链接：知道两个坐标怎么求夹角 - 百度文库（https://wenku.baidu.com/view/805 ... dWelcomeRecommand=1)

已知起点A(X1,Y1)，终点B(X2,Y2)，圆心C(X3,Y3)：

利用向量来求解，向量CA，有C->A，向量CA表示为CA=[X1-X3,Y1-Y3]，然后求其CA的模|CA|=√((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3))；

向量CB同理，C->B，向量CB表示为CB=[X2-X3,Y1-Y3]，然后求其CB的模|CB|=√((X2-X3)*(X2-X3)+(Y2-Y3)*(Y2-Y3))；

最后求两个向量的数量积，CA*CB=((X1-X3)*(X2-X3)+(Y1-Y3)*(Y2-Y3))；

由公式cos(θ)=（CA*CB)/(|CA|*|CB|)；

最后通过反余弦函数得出θ的数值。

题外话：另一种算法是用反正切函数，但是自己验证过，没有理解。附上链接地址：已知圆上两点坐标求角度-百度经验(https://jingyan.baidu.com/article/4ae03de3da4fc97ffe9e6b2c.html)。

三，代码

1.已知起点，终点，半径的代码

//已知起点 终点 半径 圆弧方向 劣弧优弧 求圆心 求弧度 求弧长
FUNCTION_BLOCK FB_Pstart_Pend_Radius
VAR_INPUT
  In_b_Start:BOOL;//开始计算

  In_lr_Start_X  :LREAL;//起点X坐标
  In_lr_Start_Y  :LREAL;//起点Y坐标
  In_lr_End_X  :LREAL;//终点X坐标
  In_lr_End_Y  :LREAL;//终点Y坐标
  In_lr_Radius  :LREAL;//半径
  In_b_Dir    :BOOL;//On:顺时针 Off:逆时针
  In_b_Inferior_Major_Arc:BOOL;//Off:劣弧 On:优弧
END_VAR
VAR_OUTPUT
  Out_b_Done:BOOL;
  Out_b_Error:BOOL;  
  
  Out_lr_Center_X:LREAL;//圆心X
  Out_lr_Center_Y:LREAL;//圆心Y
  Out_lr_Radian:LREAL;//圆心到起点 圆心到终点的弧度
  Out_lr_Degrees:LREAL;//圆心到起点 圆心到终点的度数
  Out_lr_Arc_Length:LREAL;//弧长
  
  Out_str:STRING[99];
END_VAR
VAR
  ui_loop    :UINT;//运行流程
  trig_Start:R_TRIG;//开始计算上升沿
  
  lr_LAB    :LREAL;//计算后AB之间的长度
  
  lr_Sin    :LREAL;//计算后的sin值
  lr_Cos    :LREAL;//计算后的cos值
  
  lr_Center_Mon_X:LREAL;//圆心模拟水平坐标X
  lr_Center_Mon_YP:LREAL;//圆心模拟水平坐标Y正
  lr_Center_Mon_YN:LREAL;//圆心模拟水平坐标Y负
  
  lr_Center_AZero_XP:LREAL;//圆心A坐标原点坐标X
  lr_Center_AZero_XN:LREAL;//圆心A坐标原点坐标X
  lr_Center_AZero_YP:LREAL;//圆心A坐标原点坐标Y正
  lr_Center_AZero_YN:LREAL;//圆心A坐标原点坐标Y负
  
  lr_Center_XP:LREAL;//1号圆心坐标X
  lr_Center_YP:LREAL;//1号圆心坐标Y
  lr_Center_XN:LREAL;//2号圆心坐标X
  lr_Center_YN:LREAL;//2号圆心坐标Y
  
  lr_AO_P:LREAL;//1号圆心到起点的距离
  lr_BO_P:LREAL;//1号圆心到终点的距离
  lr_AO_N:LREAL;//2号圆心到起点的距离
  lr_BO_N:LREAL;//2号圆心到终点的距离
  
  lr_Center_Start_norm:LREAL;//圆心到起点的模
  lr_Center_End_norm:LREAL;//圆心到终点的模
  
  lr_Quantityproduct_CS_CE:LREAL;//圆心到起点*圆心到终点的数量积
  
  lr_Radian_CS_CE:LREAL;//圆心到起点 圆心到终点的弧度
  
  lr_KSE:LREAL;//起点和终点 两点之间的斜率
END_VAR
VAR CONSTANT
  lr_Pi: LREAL := 3.1415926535897931;// Pi
END_VAR

//2024年11月1日 09点38分  首版本，已经可以实现基本功能



trig_Start(CLK:=In_b_Start,Q=>);
//01:The length of two points is greater than the diameter
//02:The length OF two points is equal TO 0
//04:One norm is zero
CASE  ui_loop  OF
  0://
    IF  trig_Start.Q
      THEN
        ui_loop:=10;
    END_IF
  10://置位部分标志
    Out_b_Done R= TRUE;
    Out_b_Error R= TRUE;
    Out_str := 'null';
    IF  NOT Out_b_Done  AND
      NOT Out_b_Error  AND
      Out_str='null'
      THEN
        ui_loop:=100;
    END_IF
  100://判定两点长度是否大于直径
    lr_LAB:=SQRT((In_lr_Start_X-In_lr_End_X)*(In_lr_Start_X-In_lr_End_X)+(In_lr_Start_Y-In_lr_End_Y)*(In_lr_Start_Y-In_lr_End_Y));
    IF  lr_LAB>(2*In_lr_Radius)
      THEN
        Out_str:='01:The length of two points is greater than the diameter';
        Out_b_Error S= TRUE;
        Out_b_Done S= TRUE;
        ui_loop:=0;
      ELSE
        ui_loop:=200;  
    END_IF
  200://判定两点之间的长度是否小于等于0
    IF  lr_LAB<=0
      THEN
        Out_str:='02:The length OF two points is equal TO 0';
        Out_b_Error S= TRUE;
        Out_b_Done S= TRUE;
        ui_loop:=0;
      ELSE
        ui_loop:=10000;  
    END_IF
  10000://计算A到B之间的长度
    lr_LAB:=SQRT((In_lr_Start_X-In_lr_End_X)*(In_lr_Start_X-In_lr_End_X)+(In_lr_Start_Y-In_lr_End_Y)*(In_lr_Start_Y-In_lr_End_Y));
    ui_loop:=10100;
  10100://计算模拟水平坐标往实际坐标变换的sin和cos值
    lr_Sin:=(In_lr_End_Y-In_lr_Start_Y)/lr_LAB;
    lr_Cos:=(In_lr_End_X-In_lr_Start_X)/lr_LAB;
    ui_loop:=10200;
  10200://圆心模拟水平坐标是
    lr_Center_Mon_X:=lr_LAB/2;
    lr_Center_Mon_YP:=SQRT(In_lr_Radius*In_lr_Radius-lr_Center_Mon_X*lr_Center_Mon_X);
    lr_Center_Mon_YN:=-SQRT(In_lr_Radius*In_lr_Radius-lr_Center_Mon_X*lr_Center_Mon_X);
    ui_loop:=10300;
  10300://圆心已A为坐标原点的坐标是
    lr_Center_AZero_XP:=lr_Center_Mon_X*lr_Cos-lr_Center_Mon_YP*lr_Sin;
    lr_Center_AZero_XN:=lr_Center_Mon_X*lr_Cos-lr_Center_Mon_YN*lr_Sin;
    lr_Center_AZero_YP:=lr_Center_Mon_X*lr_Sin+lr_Center_Mon_YP*lr_Cos;
    lr_Center_AZero_YN:=lr_Center_Mon_X*lr_Sin+lr_Center_Mon_YN*lr_Cos;
    ui_loop:=10400;
  10400://圆心的坐标是
    lr_Center_XP:=lr_Center_AZero_XP+In_lr_Start_X;
    lr_Center_YP:=lr_Center_AZero_YP+In_lr_Start_Y;
    
    lr_Center_XN:=lr_Center_AZero_XN+In_lr_Start_X;
    lr_Center_YN:=lr_Center_AZero_YN+In_lr_Start_Y;
    ui_loop:=11000;
  11000://计算各个圆心到起点终点的距离
    lr_AO_P:=SQRT((In_lr_Start_X-lr_Center_XP)*(In_lr_Start_X-lr_Center_XP)+(In_lr_Start_Y-lr_Center_YP)*(In_lr_Start_Y-lr_Center_YP));
    lr_BO_P:=SQRT((In_lr_End_X-lr_Center_XP)*(In_lr_End_X-lr_Center_XP)+(In_lr_End_Y-lr_Center_YP)*(In_lr_End_Y-lr_Center_YP));
    lr_AO_N:=SQRT((In_lr_Start_X-lr_Center_XN)*(In_lr_Start_X-lr_Center_XN)+(In_lr_Start_Y-lr_Center_YN)*(In_lr_Start_Y-lr_Center_YN));
    lr_BO_N:=SQRT((In_lr_End_X-lr_Center_XN)*(In_lr_End_X-lr_Center_XN)+(In_lr_End_Y-lr_Center_YN)*(In_lr_End_Y-lr_Center_YN));
    ui_loop:=11100;
  11100://验证圆心到起点终点的距离是否等于半径
    IF  lr_AO_P>=(In_lr_Radius-0.0001)  AND
      lr_AO_P<=(In_lr_Radius+0.0001)  AND
      lr_AO_N>=(In_lr_Radius-0.0001)  AND
      lr_AO_N<=(In_lr_Radius+0.0001)  AND
      lr_BO_P>=(In_lr_Radius-0.0001)  AND
      lr_BO_P<=(In_lr_Radius+0.0001)  AND
      lr_BO_N>=(In_lr_Radius-0.0001)  AND
      lr_BO_N<=(In_lr_Radius+0.0001)
      THEN
        ui_loop:=20000;
      ELSE
        Out_str:='The distance from two points to the center of the circle is not equal';
        Out_b_Error S= TRUE;
        Out_b_Done S= TRUE;
        ui_loop:=0;
    END_IF
  20000://计算起点，终点到其中一个圆心的模  
    lr_Center_Start_norm:=SQRT((In_lr_Start_X-lr_Center_XP)*(In_lr_Start_X-lr_Center_XP)+(In_lr_Start_Y-lr_Center_YP)*(In_lr_Start_Y-lr_Center_YP));
    lr_Center_End_norm:=SQRT((In_lr_End_X-lr_Center_XP)*(In_lr_End_X-lr_Center_XP)+(In_lr_End_Y-lr_Center_YP)*(In_lr_End_Y-lr_Center_YP));
    ui_loop:=20100;
  20100://计算起点，终点到圆心的数量积
    lr_Quantityproduct_CS_CE:=((In_lr_Start_X-lr_Center_XP)*(In_lr_End_X-lr_Center_XP)+(In_lr_Start_Y-lr_Center_YP)*(In_lr_End_Y-lr_Center_YP));
    ui_loop:=20200;
  20200://计算起点，终点到圆心的角度
    IF  (lr_Center_Start_norm*lr_Center_End_norm)<>0
      THEN
        lr_Radian_CS_CE:=ACOS(lr_Quantityproduct_CS_CE/(lr_Center_Start_norm*lr_Center_End_norm));
      ELSE
        Out_str:='04:One norm is zero';
        Out_b_Error S= TRUE;
        Out_b_Done S= TRUE;
        lr_Radian_CS_CE:=0;
        ui_loop:=0;
    END_IF
    ui_loop:=20300;
  20300://判定弧度是否等于Π
    IF  lr_Radian_CS_CE>=lr_Pi-0.0001  AND
      lr_Radian_CS_CE<=lr_Pi+0.0001
      THEN
        ui_loop:=30000;
      ELSE
        ui_loop:=40000;
    END_IF
  30000://起点 圆心 终点在同一条直线上
    Out_lr_Center_X:=lr_Center_XP;
    Out_lr_Center_Y:=lr_Center_YP;
    Out_lr_Radian:=lr_Radian_CS_CE;
    ui_loop:=60000;
  40000://起点 圆心 终点有一个点不在同一条直线上
    //以起点为坐标原点，如果终点落在X正坐标轴上
    IF  In_lr_Start_Y=In_lr_End_Y    AND
      In_lr_Start_X<In_lr_End_X
      THEN
        IF  lr_Center_YP>In_lr_Start_Y
          THEN//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
          ELSE//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在X负坐标轴上
    IF  In_lr_Start_Y=In_lr_End_Y    AND
      In_lr_Start_X>In_lr_End_X
      THEN
        IF  lr_Center_YP>In_lr_Start_Y
          THEN//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
          ELSE//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在Y正坐标轴上
    IF  In_lr_Start_X=In_lr_End_X    AND
      In_lr_Start_Y<In_lr_End_Y
      THEN
        IF  lr_Center_XP>In_lr_Start_X
          THEN//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
          ELSE//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
            
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在Y负坐标轴上
    IF  In_lr_Start_X=In_lr_End_X    AND
      In_lr_Start_Y>In_lr_End_Y
      THEN
        IF  lr_Center_XP>In_lr_Start_X
          THEN//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
          ELSE//顺时针
            IF  In_b_Dir
              THEN//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  //优弧  
                  ELSE
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              //逆时针
              ELSE//劣弧
                IF  NOT In_b_Inferior_Major_Arc
                  THEN
                    Out_lr_Center_X:=lr_Center_XN;
                    Out_lr_Center_Y:=lr_Center_YN;
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE//优弧
                    Out_lr_Center_X:=lr_Center_XP;
                    Out_lr_Center_Y:=lr_Center_YP;
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
        END_IF
    END_IF
    //如果起点终点都没有落在坐标轴上
    IF  In_lr_Start_X<>In_lr_End_X  AND
      In_lr_Start_Y<>In_lr_End_Y
      THEN
        lr_KSE:=(In_lr_Start_Y-In_lr_End_Y)/(In_lr_Start_X-In_lr_End_X);
        //起点终点在第一象限 一号圆心的Y值大于起点终点直线方程得出的Y
        IF  (In_lr_End_X>In_lr_Start_X)                      AND
          (In_lr_End_Y>In_lr_Start_Y)
          THEN                      
            //一号圆心点在左上方
            IF  (lr_Center_YP-In_lr_Start_Y)>(lr_KSE*(lr_Center_XP-In_lr_Start_X))
              THEN//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
              ELSE//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
            END_IF
        END_IF
        //起点终点在第二象限 中间点Y大于起点终点直线方程得出的Y 则是逆时针
        IF  (In_lr_End_X<In_lr_Start_X)                      AND
          (In_lr_End_Y>In_lr_Start_Y)
          THEN
            //一号圆心点在右上方
            IF  (lr_Center_YP-In_lr_Start_Y)>(lr_KSE*(lr_Center_XP-In_lr_Start_X))
              THEN//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
              ELSE//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
            END_IF
        END_IF
        //起点终点在第三象限 中间点Y大于起点终点直线方程得出的Y 则是逆时针
        IF  (In_lr_End_X<In_lr_Start_X)                      AND
          (In_lr_End_Y<In_lr_Start_Y)
          THEN
            //1号圆心点在左上方
            IF (lr_Center_YP-In_lr_Start_Y)>(lr_KSE*(lr_Center_XP-In_lr_Start_X))
              THEN//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
              ELSE//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
            END_IF
        END_IF
        //起点终点在第四象限 中间点Y大于起点终点直线方程得出的Y 则是顺时针
        IF  (In_lr_End_X>In_lr_Start_X)                      AND
          (In_lr_End_Y<In_lr_Start_Y)
          THEN
          //一号圆心点在右上方
            IF  (lr_Center_YP-In_lr_Start_Y)>(lr_KSE*(lr_Center_XP-In_lr_Start_X))
              THEN//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
              ELSE//顺时针
                IF  In_b_Dir
                  THEN//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      //优弧  
                      ELSE
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                  //逆时针
                  ELSE//劣弧
                    IF  NOT In_b_Inferior_Major_Arc
                      THEN
                        Out_lr_Center_X:=lr_Center_XN;
                        Out_lr_Center_Y:=lr_Center_YN;
                        Out_lr_Radian:=lr_Radian_CS_CE;
                      ELSE//优弧
                        Out_lr_Center_X:=lr_Center_XP;
                        Out_lr_Center_Y:=lr_Center_YP;
                        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                    END_IF
                END_IF
            END_IF
        END_IF
    END_IF
    ui_loop:=60000;
  60000://
    Out_lr_Degrees:=(Out_lr_Radian/lr_Pi)*180;
    Out_lr_Arc_Length:=Out_lr_Radian*In_lr_Radius;
    Out_b_Done S= TRUE;
    ui_loop:=0;
END_CASE

2.已知起点，终点，中间点的代码

//已知三点坐标 求圆心 半径 弧度 弧长 圆弧方向
//仅适用于三点都不相同
FUNCTION_BLOCK FB_Pstart_Pend_Pmiddle
VAR_INPUT
  In_b_Start:BOOL;//开始计算

  In_lr_Start_X:LREAL;// 起点X坐标
  In_lr_Start_Y:LREAL;// 起点Y坐标
  In_lr_Middle_X:LREAL;// 中间点X坐标
  In_lr_Middle_Y:LREAL;// 中间点Y坐标
  In_lr_End_X:LREAL;// 终点X坐标
  In_lr_End_Y:LREAL;// 终点Y坐标
END_VAR
VAR_OUTPUT
  Out_b_Done:BOOL;
  Out_b_Error:BOOL;  

  Out_lr_Center_X:LREAL;//圆心X
  Out_lr_Center_Y:LREAL;//圆心Y
  Out_lr_Radius:LREAL;//半径
  Out_lr_Radian:LREAL;//圆心到起点 圆心到终点的弧度
  Out_lr_Degrees:LREAL;//圆心到起点 圆心到终点的度数
  Out_lr_Arc_Length:LREAL;//弧长
  Out_b_Dir:BOOL;//On:顺时针 Off:逆时针
  
  Out_str:STRING[99];
END_VAR
VAR
  ui_loop:UINT;
  trig_Start:R_TRIG;//开始计算上升沿

  lr_KSM:LREAL;//起点和中间点 两点之间的斜率
  lr_KSE:LREAL;//起点和终点 两点之间的斜率
  lr_KSO:LREAL;//圆心和起点 两点之间的斜率
  lr_KOE:LREAL;//圆心和终点 两点之间的斜率
  
  lr_a:LREAL;//常量起点X-中间点X
  lr_b:LREAL;//常量起点Y-中间点Y
  lr_c:LREAL;//常量起点X-终点X
  lr_d:LREAL;//常量起点Y-终点Y
  lr_e:LREAL;//常量起点X*起点X-中间点X*中间点X+起点Y*起点Y-中间点Y*中间点Y
  lr_f:LREAL;//常量起点X*起点X-终点X*终点X+起点Y*起点Y-终点Y*终点Y
  
    
  lr_Start_Center:LREAL;//起点与圆心的距离
  lr_Middle_Center:LREAL;//终点与圆心的距离
  lr_End_Center:LREAL;//中间点与圆心的距离
  
  lr_Center_Start_norm:LREAL;//圆心到起点的模
  lr_Center_End_norm:LREAL;//圆心到终点的模
  lr_Center_Middle_norm:LREAL;//圆心到中间点的模
  
  lr_Quantityproduct_CS_CE:LREAL;//圆心到起点*圆心到终点的数量积
  lr_Quantityproduct_CS_CM:LREAL;//圆心到起点*圆心到中间点的数量积
  lr_Quantityproduct_CE_CM:LREAL;//圆心到终点*圆心到中间点的数量积
  
  lr_Radian_CS_CE:LREAL;//圆心到起点 圆心到终点的弧度
  lr_Radian_CS_CM:LREAL;//圆心到起点 圆心到中间点点的弧度
  lr_Radian_CE_CM:LREAL;//圆心到终点 圆心到中间点点的弧度
END_VAR
VAR CONSTANT
  lr_Pi: LREAL := 3.1415926535897931;// Pi
END_VAR

//2024年10月31日 18点30分  首版本，已经可以实现基本功能


trig_Start(CLK:=In_b_Start,Q=>);
//01:Three points on the same straight line
//02:Two points are the same
//03:The distance from three points to the center of the circle is not equal
//04:One norm is zero
CASE  ui_loop OF
  0://
    IF  trig_Start.Q
      THEN
        ui_loop:=10;
    END_IF
  10://置位部分标志
    Out_b_Done R= TRUE;
    Out_b_Error R= TRUE;
    Out_str := 'null';
    IF  NOT Out_b_Done  AND
      NOT Out_b_Error  AND
      Out_str='null'
      THEN
        ui_loop:=100;
    END_IF
  100://判定(lr_A_X-lr_B_X) (lr_A_X-lr_C_X) 是否都等于0
    IF  (In_lr_Start_X-In_lr_Middle_X)=0  AND
      (In_lr_Start_X-In_lr_End_X)=0
      THEN
        Out_str:='01:Three points on the same straight line';
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        ui_loop:=0;
    END_IF
    //有其中一个等于0
    IF  ((In_lr_Start_X-In_lr_Middle_X)=0  AND
      (In_lr_Start_X-In_lr_End_X)<>0)  OR
      ((In_lr_Start_X-In_lr_Middle_X)<>0  AND
      (In_lr_Start_X-In_lr_End_X)=0)
      THEN
        ui_loop:=400;
    END_IF
    //两个都不等于0
    IF  (In_lr_Start_X-In_lr_Middle_X)<>0  AND
      (In_lr_Start_X-In_lr_End_X)<>0
      THEN
        ui_loop:=200;
    END_IF
  200://计算三点间两两的斜率
    lr_KSM:=(In_lr_Start_Y-In_lr_Middle_Y)/(In_lr_Start_X-In_lr_Middle_X);
    lr_KSE:=(In_lr_Start_Y-In_lr_End_Y)/(In_lr_Start_X-In_lr_End_X);
    ui_loop:=300;
  300://判定三点是否在同一条直线上面
    IF  lr_KSM=lr_KSE
      THEN
        Out_str:='01:Three points on the same straight line';
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        ui_loop:=0;
      ELSE
        ui_loop:=400;
    END_IF
  400://判断是否有两点是相同的
    IF  ((In_lr_Start_X=In_lr_Middle_X)    AND
      (In_lr_Start_Y=In_lr_Middle_Y))    OR
      ((In_lr_Start_X=In_lr_End_X)      AND
      (In_lr_Start_Y=In_lr_End_Y))      OR
      ((In_lr_Middle_X=In_lr_End_X)      AND
      (In_lr_Middle_Y=In_lr_End_Y))
      THEN
        Out_str:='02:Two points are the same';
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        ui_loop:=0;
      ELSE
        ui_loop:=1000;
    END_IF
  1000://由公式：(x-x1)^2+(y-y1)^2=(x-x2)^2+(y-y2)^2
    //(x-x1)^2+(y-y1)^2=(x-x3)^2+(y-y3)^2
    
    //化简后：2(x1-x2)x+2(y1-y2)y=x1^2-x2^2+y1^2-y2^2
    //2(x1-x3)x+2(y1-y3)y=x1^2-x3^2+y1^2-y3^2
    
    //假设a=x1-x2,b=y1-y2,c=x1-x3,d=y1-y3
    //e=x1^2-x2^2+y1^2-y2^2,f=x1^2-x3^2+y1^2-y3^2
    lr_a:=In_lr_Start_X-In_lr_Middle_X;
    lr_b:=In_lr_Start_Y-In_lr_Middle_Y;
    lr_c:=In_lr_Start_X-In_lr_End_X;
    lr_d:=In_lr_Start_Y-In_lr_End_Y;
    lr_e:=In_lr_Start_X*In_lr_Start_X-In_lr_Middle_X*In_lr_Middle_X+In_lr_Start_Y*In_lr_Start_Y-In_lr_Middle_Y*In_lr_Middle_Y;
    lr_f:=In_lr_Start_X*In_lr_Start_X-In_lr_End_X*In_lr_End_X+In_lr_Start_Y*In_lr_Start_Y-In_lr_End_Y*In_lr_End_Y;
    ui_loop:=1100;
  1100://x=1/2[(bf-de)/(bc-ad)], y=1/2[(ce-af)/(bc-ad)]
    Out_lr_Center_X:=(0.5*(lr_b*lr_f-lr_d*lr_e))/(lr_b*lr_c-lr_a*lr_d);
    Out_lr_Center_Y:=(0.5*(lr_c*lr_e-lr_a*lr_f))/(lr_b*lr_c-lr_a*lr_d);
    ui_loop:=1200;
  1200://判定半径是否相同
    lr_Start_Center:=SQRT((In_lr_Start_X-Out_lr_Center_X)*(In_lr_Start_X-Out_lr_Center_X)+(In_lr_Start_Y-Out_lr_Center_Y)*(In_lr_Start_Y-Out_lr_Center_Y));
    lr_Middle_Center:=SQRT((In_lr_Middle_X-Out_lr_Center_X)*(In_lr_Middle_X-Out_lr_Center_X)+(In_lr_Middle_Y-Out_lr_Center_Y)*(In_lr_Middle_Y-Out_lr_Center_Y));
    lr_End_Center:=SQRT((In_lr_End_X-Out_lr_Center_X)*(In_lr_End_X-Out_lr_Center_X)+(In_lr_End_Y-Out_lr_Center_Y)*(In_lr_End_Y-Out_lr_Center_Y));
    ui_loop:=1300;
  1300:
    IF  lr_Start_Center<(lr_Middle_Center+0.0001)    AND
      lr_Start_Center>(lr_Middle_Center-0.0001)    AND
      lr_Start_Center<(lr_End_Center+0.0001)      AND
      lr_Start_Center>(lr_End_Center-0.0001)
      THEN
        ;
      ELSE
        Out_str:='03:The distance from three points to the center of the circle is not equal';
        Out_b_Error S= TRUE;
    END_IF
    ui_loop:=2000;
  2000://得出半径
    Out_lr_Radius:=lr_Start_Center;
    ui_loop:=3000;
  3000://圆心到起点的模和圆心到终点的模
    lr_Center_Start_norm:=SQRT((In_lr_Start_X-Out_lr_Center_X)*(In_lr_Start_X-Out_lr_Center_X)+(In_lr_Start_Y-Out_lr_Center_Y)*(In_lr_Start_Y-Out_lr_Center_Y));
    lr_Center_End_norm:=SQRT((In_lr_End_X-Out_lr_Center_X)*(In_lr_End_X-Out_lr_Center_X)+(In_lr_End_Y-Out_lr_Center_Y)*(In_lr_End_Y-Out_lr_Center_Y));
    lr_Center_Middle_norm:=SQRT((In_lr_Middle_X-Out_lr_Center_X)*(In_lr_Middle_X-Out_lr_Center_X)+(In_lr_Middle_Y-Out_lr_Center_Y)*(In_lr_Middle_Y-Out_lr_Center_Y));
    ui_loop:=3100;
  3100://计算圆心到起点和圆心到终点的数量积
    lr_Quantityproduct_CS_CE:=((In_lr_Start_X-Out_lr_Center_X)*(In_lr_End_X-Out_lr_Center_X)+(In_lr_Start_Y-Out_lr_Center_Y)*(In_lr_End_Y-Out_lr_Center_Y));
    lr_Quantityproduct_CS_CM:=((In_lr_Start_X-Out_lr_Center_X)*(In_lr_Middle_X-Out_lr_Center_X)+(In_lr_Start_Y-Out_lr_Center_Y)*(In_lr_Middle_Y-Out_lr_Center_Y));
    lr_Quantityproduct_CE_CM:=((In_lr_End_X-Out_lr_Center_X)*(In_lr_Middle_X-Out_lr_Center_X)+(In_lr_End_Y-Out_lr_Center_Y)*(In_lr_Middle_Y-Out_lr_Center_Y));
    ui_loop:=3200;
  3200://由反余弦函数求圆心到起点以及圆心到终点角度
    IF  (lr_Center_Start_norm*lr_Center_End_norm)<>0    AND
      (lr_Center_Start_norm*lr_Center_Middle_norm)<>0    AND
      (lr_Center_End_norm*lr_Center_Middle_norm)<>0  
      THEN
        lr_Radian_CS_CE:=ACOS(lr_Quantityproduct_CS_CE/(lr_Center_Start_norm*lr_Center_End_norm));
        lr_Radian_CS_CM:=ACOS(lr_Quantityproduct_CS_CM/(lr_Center_Start_norm*lr_Center_Middle_norm));
        lr_Radian_CE_CM:=ACOS(lr_Quantityproduct_CE_CM/(lr_Center_End_norm*lr_Center_Middle_norm));
    END_IF
    //判断是否有一个向量等于0
    IF  (lr_Center_Start_norm*lr_Center_End_norm)=0    OR
      (lr_Center_Start_norm*lr_Center_Middle_norm)=0  OR
      (lr_Center_End_norm*lr_Center_Middle_norm)=0
      THEN
        Out_str:='04:One norm is zero';
        Out_b_Error S= TRUE;
        lr_Radian_CS_CE:=0;
    END_IF
    ui_loop:=3300;
  3300://圆心到起点 圆心到终点的斜率是否一致 若一致 则夹角是180°
    IF  (Out_lr_Center_X-In_lr_Start_X)<>0    AND
      (In_lr_End_X-Out_lr_Center_X)<>0
      THEN
        lr_KSO:=(Out_lr_Center_Y-In_lr_Start_Y)/(Out_lr_Center_X-In_lr_Start_X);
        lr_KOE:=(In_lr_End_Y-Out_lr_Center_Y)/(In_lr_End_X-Out_lr_Center_X);
        IF  lr_KSO=lr_KOE
          THEN
            ui_loop:=4000;//起点圆心终点都在同一条直线上
          ELSE
            ui_loop:=5000;//起点圆心终点有一个不在同一条直线上°
        END_IF
    END_IF
    IF  (Out_lr_Center_X-In_lr_Start_X)=0    AND
      (In_lr_End_X-Out_lr_Center_X)=0
      THEN
        ui_loop:=4000;//起点圆心终点都在同一条直线上°
    END_IF
    IF  ((Out_lr_Center_X-In_lr_Start_X)<>0    AND
      (In_lr_End_X-Out_lr_Center_X)=0)      OR
      ((Out_lr_Center_X-In_lr_Start_X)=0      AND
      (In_lr_End_X-Out_lr_Center_X)<>0)
      THEN
        ui_loop:=5000;//起点终点非180°
    END_IF
  4000://起点圆心终点都在同一条直线上°
    Out_lr_Radian:=lr_Pi;
    ui_loop:=6000;
  5000://起点圆心终点有一个不在同一条直线上°
    //∠SOE=∠SOM+∠MOE
    IF  lr_Radian_CS_CE>((lr_Radian_CS_CM+lr_Radian_CE_CM)-0.001)  AND
      ((lr_Radian_CS_CM+lr_Radian_CE_CM)+0.001)>lr_Radian_CS_CE
      THEN
        Out_lr_Radian:=lr_Radian_CS_CE;
      ELSE
        Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
    END_IF
    ui_loop:=6000;
  6000://
    //以起点为坐标原点，如果终点落在X正坐标轴上
    IF  In_lr_Start_Y=In_lr_End_Y    AND
      In_lr_Start_X<In_lr_End_X
      THEN
        //如果中间点Y大于起点Y，则圆弧方向是顺时针
        IF  In_lr_Middle_Y>In_lr_Start_Y
          THEN
            Out_b_Dir S= TRUE;
          ELSE
            Out_b_Dir R= TRUE;
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在X负坐标轴上
    IF  In_lr_Start_Y=In_lr_End_Y    AND
      In_lr_Start_X>In_lr_End_X
      THEN
        //如果中间点Y大于起点Y，则圆弧方向是逆时针
        IF  In_lr_Middle_Y>In_lr_Start_Y
          THEN
            Out_b_Dir R= TRUE;
          ELSE
            Out_b_Dir S= TRUE;
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在Y正坐标轴上
    IF  In_lr_Start_X=In_lr_End_X    AND
      In_lr_Start_Y<In_lr_End_Y
      THEN
        //如果中间点X大于起点X，则圆弧方向是逆时针
        IF  In_lr_Middle_X>In_lr_Start_X
          THEN
            Out_b_Dir R= TRUE;
          ELSE
            Out_b_Dir S= TRUE;
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在Y负坐标轴上
    IF  In_lr_Start_X=In_lr_End_X    AND
      In_lr_Start_Y>In_lr_End_Y
      THEN
        //如果中间点X大于起点X，则圆弧方向是顺时针
        IF  In_lr_Middle_X>In_lr_Start_X
          THEN
            Out_b_Dir S= TRUE;
          ELSE
            Out_b_Dir R= TRUE;
        END_IF
    END_IF
    //如果起点终点都没有落在坐标轴上
    IF  In_lr_Start_X<>In_lr_End_X  AND
      In_lr_Start_Y<>In_lr_End_Y
      THEN
        lr_KSE:=(In_lr_Start_Y-In_lr_End_Y)/(In_lr_Start_X-In_lr_End_X);
        //起点终点在第一象限 中间点Y大于起点终点直线方程得出的Y 则是顺时针
        IF  (In_lr_End_X>In_lr_Start_X)                      AND
          (In_lr_End_Y>In_lr_Start_Y)                      AND
          (In_lr_Middle_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Middle_X-In_lr_Start_X))
          THEN
            Out_b_Dir S= TRUE;
          ELSE
            Out_b_Dir R= TRUE;
        END_IF
        //起点终点在第二象限 中间点Y大于起点终点直线方程得出的Y 则是逆时针
        IF  (In_lr_End_X<In_lr_Start_X)                      AND
          (In_lr_End_Y>In_lr_Start_Y)                      AND
          (In_lr_Middle_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Middle_X-In_lr_Start_X))
          THEN
            Out_b_Dir R= TRUE;
          ELSE
            Out_b_Dir S= TRUE;
        END_IF
        //起点终点在第三象限 中间点Y大于起点终点直线方程得出的Y 则是逆时针
        IF  (In_lr_End_X<In_lr_Start_X)                      AND
          (In_lr_End_Y<In_lr_Start_Y)                      AND
          (In_lr_Middle_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Middle_X-In_lr_Start_X))
          THEN
            Out_b_Dir R= TRUE;
          ELSE
            Out_b_Dir S= TRUE;
        END_IF
        //起点终点在第四象限 中间点Y大于起点终点直线方程得出的Y 则是顺时针
        IF  (In_lr_End_X>In_lr_Start_X)                      AND
          (In_lr_End_Y<In_lr_Start_Y)                      AND
          (In_lr_Middle_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Middle_X-In_lr_Start_X))
          THEN
            Out_b_Dir S= TRUE;
          ELSE
            Out_b_Dir R= TRUE;
        END_IF
    END_IF
    ui_loop:=10000;
  10000://弧度转换成度数
    Out_lr_Degrees:=(Out_lr_Radian/lr_Pi)*180;
    Out_lr_Arc_Length:=Out_lr_Radian*Out_lr_Radius;
    Out_b_Done S= TRUE;
    ui_loop:=0;
END_CASE

3.已知起点，终点，圆心的代码

FUNCTION_BLOCK FB_Pstart_Pend_Pcenter
VAR_INPUT
  In_b_Start:BOOL;//开始计算

  In_lr_Start_X  :LREAL;//起点X坐标
  In_lr_Start_Y  :LREAL;//起点Y坐标
  In_lr_End_X  :LREAL;//终点X坐标
  In_lr_End_Y  :LREAL;//终点Y坐标
  In_lr_Center_X  :LREAL;//中间点X坐标
  In_lr_Center_Y  :LREAL;//中间点Y坐标
  In_b_Dir    :BOOL;//On:顺时针 Off:逆时针
END_VAR
VAR_OUTPUT
  Out_b_Done:BOOL;
  Out_b_Error:BOOL;  
  
  Out_lr_Radius  :LREAL;//半径
  
  Out_lr_Radian:LREAL;//圆心到起点 圆心到终点的弧度
  Out_lr_Degrees:LREAL;//圆心到起点 圆心到终点的度数
  Out_lr_Arc_Length:LREAL;//弧长
  
  Out_str:STRING[99];
END_VAR
VAR
  ui_loop    :UINT;//运行流程
  trig_Start:R_TRIG;//开始计算上升沿
  
  lr_LSO    :LREAL;//计算起点到圆心的距离
  lr_LEO    :LREAL;//计算终点到圆心的距离
  
  lr_Center_Start_norm:LREAL;//圆心到起点的模
  lr_Center_End_norm:LREAL;//圆心到终点的模
  
  lr_Quantityproduct_CS_CE:LREAL;//圆心到起点*圆心到终点的数量积
  
  lr_Radian_CS_CE:LREAL;//圆心到起点 圆心到终点的弧度
  
  lr_KSE:LREAL;//起点和终点 两点之间的斜率
END_VAR
VAR CONSTANT
  lr_Pi: LREAL := 3.1415926535897931;// Pi
END_VAR

//2024年11月2日 11点10分  首版本，已经可以实现基本功能



trig_Start(CLK:=In_b_Start,Q=>);
//01:The distance from two points to the center of the circle is different
//04:One norm is zero
CASE  ui_loop  OF
  0://
    IF  trig_Start.Q
      THEN
        ui_loop:=10;
    END_IF
  10://置位部分标志
    Out_b_Done R= TRUE;
    Out_b_Error R= TRUE;
    Out_str := 'null';
    IF  NOT Out_b_Done  AND
      NOT Out_b_Error  AND
      Out_str='null'
      THEN
        ui_loop:=100;
    END_IF
  100://判定两点到圆心的距离是否一致
    lr_LSO:=SQRT((In_lr_Center_X-In_lr_End_X)*(In_lr_Center_X-In_lr_End_X)+(In_lr_Center_Y-In_lr_End_Y)*(In_lr_Center_Y-In_lr_End_Y));
    lr_LEO:=SQRT((In_lr_Center_X-In_lr_Start_X)*(In_lr_Center_X-In_lr_Start_X)+(In_lr_Center_Y-In_lr_Start_Y)*(In_lr_Center_Y-In_lr_Start_Y));
    IF  lr_LSO>=(lr_LEO-0.001)    AND
      lr_LSO<=(lr_LEO+0.001)
      THEN
        ui_loop:=20000;
      ELSE
        Out_str:='01:The distance from two points to the center of the circle is different';
        Out_b_Error S= TRUE;
        Out_b_Done S= TRUE;
        ui_loop:=0;
    END_IF
  20000://计算起点，终点到其中一个圆心的模  
    Out_lr_Radius:=lr_LSO;
    lr_Center_Start_norm:=SQRT((In_lr_Start_X-In_lr_Center_X)*(In_lr_Start_X-In_lr_Center_X)+(In_lr_Start_Y-In_lr_Center_Y)*(In_lr_Start_Y-In_lr_Center_Y));
    lr_Center_End_norm:=SQRT((In_lr_End_X-In_lr_Center_X)*(In_lr_End_X-In_lr_Center_X)+(In_lr_End_Y-In_lr_Center_Y)*(In_lr_End_Y-In_lr_Center_Y));
    ui_loop:=20100;
  20100://计算起点，终点到圆心的数量积
    lr_Quantityproduct_CS_CE:=((In_lr_Start_X-In_lr_Center_X)*(In_lr_End_X-In_lr_Center_X)+(In_lr_Start_Y-In_lr_Center_Y)*(In_lr_End_Y-In_lr_Center_Y));
    ui_loop:=20200;
  20200://计算起点，终点到圆心的角度
    IF  (lr_Center_Start_norm*lr_Center_End_norm)<>0
      THEN
        lr_Radian_CS_CE:=ACOS(lr_Quantityproduct_CS_CE/(lr_Center_Start_norm*lr_Center_End_norm));
      ELSE
        Out_str:='04:One norm is zero';
        Out_b_Error S= TRUE;
        Out_b_Done S= TRUE;
        lr_Radian_CS_CE:=0;
        ui_loop:=0;
    END_IF
    ui_loop:=20300;
  20300://判定弧度是否等于Π
    IF  (lr_Radian_CS_CE>=lr_Pi-0.0001  AND
      lr_Radian_CS_CE<=lr_Pi+0.0001)
      THEN
        ui_loop:=30000;
      ELSIF  (lr_Radian_CS_CE>=-0.0001  AND
          lr_Radian_CS_CE<=0.0001)
          THEN
            ui_loop:=31000;
      ELSE
        ui_loop:=40000;
    END_IF
  30000://起点 圆心 终点在同一条直线上
    In_lr_Center_X:=In_lr_Center_X;
    In_lr_Center_Y:=In_lr_Center_Y;
    Out_lr_Radian:=lr_Radian_CS_CE;
    ui_loop:=60000;
  31000://起点 圆心 终点在同一条直线上 起点终点重合
    In_lr_Center_X:=In_lr_Center_X;
    In_lr_Center_Y:=In_lr_Center_Y;
    Out_lr_Radian:=2*lr_Pi;
    ui_loop:=60000;
  40000://起点 圆心 终点有一个点不在同一条直线上
    //以起点为坐标原点，如果终点落在X正坐标轴上
    IF  In_lr_Start_Y=In_lr_End_Y    AND
      In_lr_Start_X<In_lr_End_X
      THEN
        IF  In_lr_Center_Y>In_lr_Start_Y
          THEN
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=lr_Radian_CS_CE;
            END_IF
          ELSE
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
            END_IF
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在X负坐标轴上
    IF  In_lr_Start_Y=In_lr_End_Y    AND
      In_lr_Start_X>In_lr_End_X
      THEN
        IF  In_lr_Center_Y>In_lr_Start_Y
          THEN
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
            END_IF
          ELSE
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=lr_Radian_CS_CE;
            END_IF
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在Y正坐标轴上
    IF  In_lr_Start_X=In_lr_End_X    AND
      In_lr_Start_Y<In_lr_End_Y
      THEN
        IF  In_lr_Center_X>In_lr_Start_X
          THEN
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
            END_IF
          ELSE
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=lr_Radian_CS_CE;
            END_IF
        END_IF
    END_IF
    //以起点为坐标原点，如果终点落在Y负坐标轴上
    IF  In_lr_Start_X=In_lr_End_X    AND
      In_lr_Start_Y>In_lr_End_Y
      THEN
        IF  In_lr_Center_X>In_lr_Start_X
          THEN
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=lr_Radian_CS_CE;
            END_IF
          ELSE
            IF  In_b_Dir
              THEN
                Out_lr_Radian:=lr_Radian_CS_CE;
              ELSE
                Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
            END_IF
        END_IF
    END_IF
    //如果起点终点都没有落在坐标轴上
    IF  In_lr_Start_X<>In_lr_End_X  AND
      In_lr_Start_Y<>In_lr_End_Y
      THEN
        lr_KSE:=(In_lr_Start_Y-In_lr_End_Y)/(In_lr_Start_X-In_lr_End_X);
        //起点终点在第一象限 一号圆心的Y值大于起点终点直线方程得出的Y
        IF  (In_lr_End_X>In_lr_Start_X)                      AND
          (In_lr_End_Y>In_lr_Start_Y)
          THEN                      
            //圆心点在左上方
            IF  (In_lr_Center_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Center_X-In_lr_Start_X))
              THEN
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=lr_Radian_CS_CE;
                END_IF
              ELSE
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
        END_IF
        //起点终点在第二象限 中间点Y大于起点终点直线方程得出的Y 则是逆时针
        IF  (In_lr_End_X<In_lr_Start_X)                      AND
          (In_lr_End_Y>In_lr_Start_Y)
          THEN
            //圆心点在右上方
            IF  (In_lr_Center_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Center_X-In_lr_Start_X))
              THEN
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              ELSE
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=lr_Radian_CS_CE;
                END_IF
            END_IF
        END_IF
        //起点终点在第三象限 中间点Y大于起点终点直线方程得出的Y 则是逆时针
        IF  (In_lr_End_X<In_lr_Start_X)                      AND
          (In_lr_End_Y<In_lr_Start_Y)
          THEN
            //1号圆心点在左上方
            IF (In_lr_Center_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Center_X-In_lr_Start_X))
              THEN
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
              ELSE
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=lr_Radian_CS_CE;
                END_IF
            END_IF
        END_IF
        //起点终点在第四象限 中间点Y大于起点终点直线方程得出的Y 则是顺时针
        IF  (In_lr_End_X>In_lr_Start_X)                      AND
          (In_lr_End_Y<In_lr_Start_Y)
          THEN
          //一号圆心点在右上方
            IF  (In_lr_Center_Y-In_lr_Start_Y)>(lr_KSE*(In_lr_Center_X-In_lr_Start_X))
              THEN
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=lr_Radian_CS_CE;
                END_IF
              ELSE
                IF  In_b_Dir
                  THEN
                    Out_lr_Radian:=lr_Radian_CS_CE;
                  ELSE
                    Out_lr_Radian:=2*lr_Pi-lr_Radian_CS_CE;
                END_IF
            END_IF
        END_IF
    END_IF
    ui_loop:=60000;
  60000://
    Out_lr_Degrees:=(Out_lr_Radian/lr_Pi)*180;
    Out_lr_Arc_Length:=Out_lr_Radian*Out_lr_Radius;
    Out_b_Done S= TRUE;
    ui_loop:=0;
END_CASE

4.点胶步序结构体变量

//步序程序的结构体
TYPE dut_GluePro_Step :
STRUCT
  b_Helix_Line                    :  BOOL        ;//off:直线插补 on:螺旋插补
  lr_Speed_Ratio                    :  LREAL        ;//运行速度 比值
  lr_Corner_Auxiliary_Circle_Radius          :  LREAL        ;//拐角辅助圆的半径
  (****************************************点胶相关性的参数****************************************)
  b_Glue                        :  BOOL        ;//off:关闭点胶 on:开启点胶
  
  ui_Ahead_OpenGlue                  :  UINT        ;//在打胶的起始位置 提前开胶的时长(为0不提前开胶)
  lr_Delay_OpenGlue                  :  LREAL        ;//在打胶的起始位置 延时开胶的位置(为0不提前关胶) 单位0.01mm
  lr_Ahead_CloseGlue                  :  LREAL        ;//在打胶的结束位置 提前关胶的位置(为0不提前关胶) 单位0.01mm
  
  lr_X_EndPos                      :  LREAL        ;//X轴的打胶结束位置
  lr_Y_EndPos                      :  LREAL        ;//Y轴的打胶结束位置
  lr_Z_EndPos                      :  LREAL        ;//Z轴的打胶结束位置
  (****************************************圆弧相关性的参数****************************************)
  ui_Helix_Mode                    :  UINT        ;//螺旋的模式 0:起点 终点 半径;1:起点 中点 终点;2:起点 圆心 终点
  
  b_Arc_Direction                    :  BOOL        ;//off:CW 时针  on:CWW 逆时针 当螺旋模式ui_Helix_Mode是0 2时候可以选择
  b_Inferior_Major_Arc                :  BOOL        ;//Off:劣弧 On:优弧 当螺旋模式ui_Helix_Mode是1时候可以选择

  
  lr_X_Center_Circle                  :  LREAL        ;//在ui_Helix_Mode=2既是螺旋的模式=2时候 X轴的圆心
  lr_Y_Center_Circle                  :  LREAL        ;//在ui_Helix_Mode=2既是螺旋的模式=2时候 Y轴的圆心
  
  lr_X_MidPoint                    :  LREAL        ;//在ui_Helix_Mode=1既是螺旋的模式=1时候 X轴的中点
  lr_Y_MidPoint                    :  LREAL        ;//在ui_Helix_Mode=1既是螺旋的模式=1时候 Y轴的中点
  
  lr_Radius                      :  LREAL        ;//在ui_Helix_Mode=0既是螺旋的模式=0时候 圆的半径
END_STRUCT
END_TYPE

5.插补功能块

FUNCTION_BLOCK FB_Glue_Dispensing_Trajectory
VAR_INPUT
  In_b_Start                      :  BOOL                  ;//开始计算
  
  In_ui_GlueStep_Num                  :  UINT                  ;//点胶步数
  In_dut_GluePro_Step                  :  ARRAY[0..19] OF dut_GluePro_Step    ;//点胶程序的设定
  
END_VAR
VAR_OUTPUT
  Out_b_Done                      :  BOOL                  ;//运行完成
  Out_b_Busy                      :  BOOL                  ;//功能块计算中
  Out_b_Error                      :  BOOL                  ;//运行失败标志
  Out_ui_Error_Step                  :  UINT                  ;//运行失败步数
  Out_str_Error_FB                  :  STRING[99]                ;//运行失败功能块
  Out_str_Error                    :  STRING[99]                ;//运行失败代码
  
  Out_udi_Total                    :  UDINT                  ;//总的凸轮点数
  Out_ui_CamPointNum                  :  ARRAY[0..19] OF UINT          ;//每个点胶步数需要的凸轮点数
  Out_lr_Length                    :  ARRAY [0..20] OF LREAL          ;// 插补的长度
  
  Out_ui_Total_Start_GluePoint            :  ARRAY[0..20] OF UINT          ;//总点数中该步数点胶起始点
  Out_ui_Total_End_GluePoint              :  ARRAY[0..20] OF UINT          ;//总点数中该步数点胶终点
  
  Out_lr_X                      :  ARRAY[0..32766] OF LREAL        ;
  Out_lr_Y                      :  ARRAY[0..32766] OF LREAL        ;
  Out_lr_Z                      :  ARRAY[0..32766] OF LREAL        ;
END_VAR
VAR
  ui_Loop                        :  UINT                  ;//步序流程
  trig_Start                      :  R_TRIG                  ;//开始计算上升沿
  ui_GlueStep_Count                  :  UINT                  ;//已计算步序计数
  
  (****************************圆弧插补****************************)
  lr_Arc_Length_XY                  :  ARRAY [0..20] OF LREAL          ;// 插补的XY面上的弧线长度
  lr_Arc_Radian_XY                  :  ARRAY [0..20] OF LREAL          ;// 插补的XY面上的弧线弧度
  b_Arc_Dir_XY                    :  ARRAY [0..20] OF BOOL          ;// 插补的XY面上的弧线的方向 On:顺时针
  lr_Center_X                      :  ARRAY [0..20] OF LREAL          ;// 圆心的X座标
  lr_Center_Y                      :  ARRAY [0..20] OF LREAL          ;// 圆心的Y座标
  lr_End_X                      :  ARRAY [0..20] OF LREAL          ;// 终点的X座标
  lr_End_Y                      :  ARRAY [0..20] OF LREAL          ;// 终点的Y座标
  lr_Arc_Radius_XY                  :  ARRAY [0..20] OF LREAL          ;// 插补的XY面上的圆的半径
  ui_Radian_Div                    :  ARRAY [0..20] OF UINT          ;// 弧度的细分
  //圆弧插补过程中以原点为坐标中心的相关设置变量
  lr_Start_X_OriginisCenter              :  LREAL                  ;// 圆弧插补过程中以原点为坐标中心的相关设置变量:起点的X座标
  lr_Start_Y_OriginisCenter              :  LREAL                  ;// 起点的Y座标
  lr_Radian_Ps_XP_OriginisCenter            :  LREAL                  ;//  起点和X正坐标轴的角度
  lr_Radian_Ps_YP_OriginisCenter            :  LREAL                  ;//  起点和Y正坐标轴的角度
  lr_Radian_Ps_XN_OriginisCenter            :  LREAL                  ;//  起点和X负坐标轴的角度
  lr_Radian_Ps_YN_OriginisCenter            :  LREAL                  ;//  起点和X负坐标轴的角度
  //X点，Y点，Z点坐标数组的下标
  ui_CamPoint_Index                  :  UINT                  ;//X点，Y点，Z点坐标数组的下标
  //当前步数之前点胶步数凸轮点数的累计
  ui_Total_CamPoint_Num_After              :  ARRAY[0..20] OF UINT          ;//之前点胶步数凸轮点数的累计
  //当前步数之前点胶步数凸轮点数+当前点胶步数的凸轮点数的累计
  ui_Total_CamPoint_Num                :  ARRAY[0..20] OF UINT          ;//当前步数+之前点胶步数凸轮点数的累计
  //
  ui_For_i                      :  UINT                  ;//遍历过程中，遍历次数的计算
  ui_For_i_CamPoint                  :  UINT                  ;//遍历过程中，遍历次数的计算
  //每个点胶步数的终点坐标
  lr_PrePoint_X                    :  ARRAY[0..20] OF LREAL          ;//上一点X最后一点坐标
  lr_PrePoint_Y                    :  ARRAY[0..20] OF LREAL          ;//上一点Y最后一点坐标
  lr_PrePoint_Z                    :  ARRAY[0..20] OF LREAL          ;//上一点Z最后一点坐标
  //三个求弧长的功能块
  FB_Pstart_Pend_Pmiddle_1              :FB_Pstart_Pend_Pmiddle            ;
  FB_Pstart_Pend_Radius_1                :FB_Pstart_Pend_Radius            ;
  FB_Pstart_Pend_Pcenter_1              :FB_Pstart_Pend_Pcenter            ;
  //出胶设置起点终点
  ui_Step_Start_GluePoint                :  ARRAY[0..20] OF UINT          ;//当前步序该点胶起始点
  ui_Step_End_GluePoint                :  ARRAY[0..20] OF UINT          ;//当前步序该点胶终点
  
  lr_test:LREAL;
END_VAR
VAR CONSTANT
  lr_mm_CamPoint                    :  LREAL := 100              ;//1mm需要多少凸轮点数
  lr_Pi                        :  LREAL := 3.1415926535897931        ;// Pi
  udi_CamPoint_UpLimit                :  UDINT := 32767              ;//凸轮
END_VAR

trig_Start(CLK:=In_b_Start,Q=>);

//'01:The number of points exceeds 32767'
//'02:The length of this step is 0'
//'03:The speed of this step is 0'

FB_Pstart_Pend_Radius_1(
  In_b_Start:= , 
  In_lr_Start_X:= 0, 
  In_lr_Start_Y:= 0, 
  In_lr_End_X:= , 
  In_lr_End_Y:= , 
  In_lr_Radius:= , 
  In_b_Dir:= , 
  In_b_Inferior_Major_Arc:= , 
  Out_b_Done=> , 
  Out_b_Error=> , 
  Out_lr_Center_X=> , 
  Out_lr_Center_Y=> , 
  Out_lr_Radian=> , 
  Out_lr_Degrees=> , 
  Out_lr_Arc_Length=> , 
  Out_str=> );
FB_Pstart_Pend_Pmiddle_1(
  In_b_Start:= , 
  In_lr_Start_X:= 0, 
  In_lr_Start_Y:= 0, 
  In_lr_Middle_X:= , 
  In_lr_Middle_Y:= , 
  In_lr_End_X:= , 
  In_lr_End_Y:= , 
  Out_b_Done=> , 
  Out_b_Error=> , 
  Out_lr_Center_X=> , 
  Out_lr_Center_Y=> , 
  Out_lr_Radius=> , 
  Out_lr_Radian=> , 
  Out_lr_Degrees=> , 
  Out_lr_Arc_Length=> , 
  Out_b_Dir=> , 
  Out_str=> );
FB_Pstart_Pend_Pcenter_1(
  In_b_Start:= , 
  In_lr_Start_X:= 0, 
  In_lr_Start_Y:= 0, 
  In_lr_End_X:= , 
  In_lr_End_Y:= , 
  In_lr_Center_X:= , 
  In_lr_Center_Y:= , 
  In_b_Dir:= , 
  Out_b_Done=> , 
  Out_b_Error=> , 
  Out_lr_Radius=> , 
  Out_lr_Radian=> , 
  Out_lr_Degrees=> , 
  Out_lr_Arc_Length=> , 
  Out_str=> );
CASE ui_Loop OF
  0:
    IF  trig_Start.Q
      THEN
        ui_loop:=10;
    END_IF;
  10://置位部分标志
    Out_b_Busy S= TRUE;
    Out_b_Done R= TRUE;
    Out_b_Error := 0;
    Out_ui_Error_Step := 0;
    Out_str_Error_FB := 'null';
    Out_str_Error := 'null';
    
    Out_udi_Total:=0;
    
    ui_GlueStep_Count := 0;
    ui_CamPoint_Index := 0;
    Out_lr_X[0]:=0;
    Out_lr_Y[0]:=0;
    Out_lr_Z[0]:=0;
    
    
    FB_Pstart_Pend_Radius_1.In_b_Start R= TRUE;
    FB_Pstart_Pend_Pmiddle_1.In_b_Start R= TRUE;
    FOR ui_For_i:=0 TO 20 BY 1 DO
      Out_ui_CamPointNum[ui_For_i]:=0;
      Out_lr_Length[ui_For_i]:=0;
      
      Out_ui_Total_Start_GluePoint[ui_For_i]:=0;
      Out_ui_Total_End_GluePoint[ui_For_i]:=0;
      
      lr_PrePoint_X[ui_For_i]:=0;
      lr_PrePoint_Y[ui_For_i]:=0;
      lr_PrePoint_Z[ui_For_i]:=0;
      
      ui_Total_CamPoint_Num_After[ui_For_i]:=0;
      ui_Total_CamPoint_Num[ui_For_i]:=0;
      
      ui_Step_Start_GluePoint[ui_For_i]:=0;
      ui_Step_End_GluePoint[ui_For_i]:=0;
    END_FOR
    
    FOR ui_For_i_CamPoint:=0 TO UDINT_TO_UINT(udi_CamPoint_UpLimit) BY 1 DO
      Out_lr_X[ui_For_i_CamPoint]:=0;
      Out_lr_Y[ui_For_i_CamPoint]:=0;
      Out_lr_Z[ui_For_i_CamPoint]:=0;
    END_FOR
    
    ui_Total_CamPoint_Num_After[0]:=1;
    IF  NOT Out_b_Done      AND
      Out_b_Error=0      AND
      ui_GlueStep_Count=0    AND
      ui_CamPoint_Index=0    AND
      lr_PrePoint_X[0]=0    AND
      lr_PrePoint_Y[0]=0    AND
      lr_PrePoint_Z[0]=0    AND
      Out_lr_X[0]=0      AND
      Out_lr_Y[0]=0      AND
      Out_lr_Z[0]=0      AND
      ui_Total_CamPoint_Num_After[0]=1
      THEN
        ui_loop:=1000;
    END_IF
  1000://判定当前的步数是否超过上限
    IF  In_ui_GlueStep_Num>ui_GlueStep_Count
      THEN
        ui_loop:=10000;
      ELSE//运算结束
        ui_loop:=60000;
    END_IF
  10000://判定当前的步序是直线插补还是螺旋插补
    ui_For_i:=1;
    IF  ui_For_i=1
      THEN
        //直线插补
        IF  NOT In_dut_GluePro_Step[ui_GlueStep_Count].b_Helix_Line
          THEN
            ui_loop:=20000;
          //螺旋插补
          ELSE
            ui_loop:=30000;
        END_IF
    END_IF
  20000://直线插补:计算直线的长度
    Out_lr_Length[ui_GlueStep_Count]:=SQRT(  In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_EndPos*In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_EndPos+
                    In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_EndPos*In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_EndPos+
                    In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos*In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos
              );
    //判定直线的长度是否大于0，若非大于0则不执行
    IF  Out_lr_Length[ui_GlueStep_Count]>0
      THEN
        ui_loop:=20100;
      ELSE
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        Out_b_Busy R= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Glue_Dispensing_Trajectory';
        Out_str_Error:='02:The length of this step is 0';
        ui_loop:=0;
    END_IF
  20100://直线插补:根据速率 和 长度 计算所需要的点数
    IF In_dut_GluePro_Step[ui_GlueStep_Count].lr_Speed_Ratio<>0 THEN
      Out_ui_CamPointNum[ui_GlueStep_Count] := LREAL_TO_UINT(lr_mm_CamPoint * (Out_lr_Length[ui_GlueStep_Count]/1000) * (1/(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Speed_Ratio/1000)));
      ui_Total_CamPoint_Num[ui_GlueStep_Count] :=ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+Out_ui_CamPointNum[ui_GlueStep_Count];
      ui_loop:=20150;
    ELSE
      Out_b_Done S= TRUE;
      Out_b_Error S= TRUE;
      Out_b_Busy R= TRUE;
      Out_ui_Error_Step:=ui_GlueStep_Count;
      Out_str_Error_FB:='FB_Glue_Dispensing_Trajectory';
      Out_str_Error:='03:The speed of this step is 0';
      ui_loop:=0;
    END_IF
  20150://判定当前步数有无超过设定上限
    IF  ui_Total_CamPoint_Num[ui_GlueStep_Count]>(udi_CamPoint_UpLimit-100)
      THEN
        Out_b_Done S= TRUE;
        Out_b_Busy R= TRUE;
        Out_b_Error S= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Glue_Dispensing_Trajectory';
        Out_str_Error:='01:The number of points exceeds 32767';
        ui_loop:=1000;
      ELSE
        ui_Loop:=20200;  
    END_IF    
  20200://直线插补:遍历Out_ui_CamPointNum[ui_GlueStep_Count]次写入需要的
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  30000://圆弧插补:判定圆弧是以哪种模式进行画圆
    //螺旋的模式 0:起点 终点 半径;1:起点 中点 终点;2:起点 圆心 终点
    //起点 终点 半径
    IF  In_dut_GluePro_Step[ui_GlueStep_Count].ui_Helix_Mode=0
      THEN
        ui_loop:=31000;
    END_IF
    //起点 中点 终点
    IF  In_dut_GluePro_Step[ui_GlueStep_Count].ui_Helix_Mode=1
      THEN
        ui_loop:=32000;
    END_IF
    //起点 圆心 终点
    IF  In_dut_GluePro_Step[ui_GlueStep_Count].ui_Helix_Mode=2
      THEN
        ui_loop:=33000;
    END_IF
  31000://圆弧插补起点 终点 半径模式:
    //写入终点 以及半径
    FB_Pstart_Pend_Radius_1.In_lr_End_X:=In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_EndPos;
    FB_Pstart_Pend_Radius_1.In_lr_End_Y:=In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_EndPos;
    FB_Pstart_Pend_Radius_1.In_lr_Radius:=In_dut_GluePro_Step[ui_GlueStep_Count].lr_Radius;
    FB_Pstart_Pend_Radius_1.In_b_Dir:=In_dut_GluePro_Step[ui_GlueStep_Count].b_Arc_Direction;
    FB_Pstart_Pend_Radius_1.In_b_Inferior_Major_Arc:= In_dut_GluePro_Step[ui_GlueStep_Count].b_Inferior_Major_Arc;
    ui_loop:=31100;
  31100://圆弧插补起点 终点 半径模式:触发进行计算
    FB_Pstart_Pend_Radius_1.In_b_Start S= TRUE;
    IF  FB_Pstart_Pend_Radius_1.In_b_Start    AND
      FB_Pstart_Pend_Radius_1.Out_b_Done    AND
      NOT FB_Pstart_Pend_Radius_1.Out_b_Error
      THEN
        ui_loop:=31200;
    END_IF
    //计算错误
    IF  FB_Pstart_Pend_Radius_1.In_b_Start    AND
      FB_Pstart_Pend_Radius_1.Out_b_Done    AND
      FB_Pstart_Pend_Radius_1.Out_b_Error
      THEN
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        Out_b_Busy R= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Pstart_Pend_Radius';
        Out_str_Error:=FB_Pstart_Pend_Radius_1.Out_str;
        ui_loop:=0;
    END_IF
  31200://圆弧插补起点 终点 半径模式:输出计算结果
    FB_Pstart_Pend_Radius_1.In_b_Start R= TRUE;
    lr_Arc_Length_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.Out_lr_Arc_Length;
    lr_Arc_Radian_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.Out_lr_Radian;
    b_Arc_Dir_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.In_b_Dir;
    lr_Center_X[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.Out_lr_Center_X;
    lr_Center_Y[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.Out_lr_Center_Y;
    lr_End_X[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.In_lr_End_X;
    lr_End_Y[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.In_lr_End_Y;
    lr_Arc_Radius_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Radius_1.In_lr_Radius;
    ui_loop:=40000;
  32000://圆弧插补起点 中点 终点模式:
    //写入中间点 终点
    FB_Pstart_Pend_Pmiddle_1.In_lr_End_X:=In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_EndPos;
    FB_Pstart_Pend_Pmiddle_1.In_lr_End_Y:=In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_EndPos;
    FB_Pstart_Pend_Pmiddle_1.In_lr_Middle_X:=In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_MidPoint;
    FB_Pstart_Pend_Pmiddle_1.In_lr_Middle_Y:=In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_MidPoint;
    ui_loop:=32100;
  32100://圆弧插补起点 中点 终点模式:触发进行计算
    FB_Pstart_Pend_Pmiddle_1.In_b_Start S= TRUE;
    IF  FB_Pstart_Pend_Pmiddle_1.In_b_Start    AND
      FB_Pstart_Pend_Pmiddle_1.Out_b_Done    AND
      NOT FB_Pstart_Pend_Pmiddle_1.Out_b_Error
      THEN
        ui_loop:=32200;
    END_IF
    IF  FB_Pstart_Pend_Pmiddle_1.In_b_Start    AND
      FB_Pstart_Pend_Pmiddle_1.Out_b_Done    AND
      FB_Pstart_Pend_Pmiddle_1.Out_b_Error
      THEN
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        Out_b_Busy R= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Pstart_Pend_Pmiddle';
        Out_str_Error:=FB_Pstart_Pend_Pmiddle_1.Out_str;
        ui_loop:=0;
    END_IF
  32200://圆弧插补起点 中点 终点模式:输出计算结果
    FB_Pstart_Pend_Pmiddle_1.In_b_Start R= TRUE;
    lr_Arc_Length_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.Out_lr_Arc_Length;
    lr_Arc_Radian_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.Out_lr_Radian;
    b_Arc_Dir_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.Out_b_Dir;
    lr_Center_X[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.Out_lr_Center_X;
    lr_Center_Y[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.Out_lr_Center_Y;
    lr_End_X[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.In_lr_End_X;
    lr_End_Y[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.In_lr_End_Y;
    lr_Arc_Radius_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pmiddle_1.Out_lr_Radius;
    ui_loop:=40000;
  33000://圆弧插补起点 圆心 终点模式:
    //写入圆心 写入终点
    FB_Pstart_Pend_Pcenter_1.In_lr_End_X := In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_EndPos;
    FB_Pstart_Pend_Pcenter_1.In_lr_End_Y := In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_EndPos;
    FB_Pstart_Pend_Pcenter_1.In_lr_Center_X := In_dut_GluePro_Step[ui_GlueStep_Count].lr_X_Center_Circle;
    FB_Pstart_Pend_Pcenter_1.In_lr_Center_Y := In_dut_GluePro_Step[ui_GlueStep_Count].lr_Y_Center_Circle;
    FB_Pstart_Pend_Pcenter_1.In_b_Dir := In_dut_GluePro_Step[ui_GlueStep_Count].b_Arc_Direction;
    ui_loop:=33100;
  33100://圆弧插补起点 圆心 终点模式:触发开始计算
    FB_Pstart_Pend_Pcenter_1.In_b_Start S= TRUE;
    IF  FB_Pstart_Pend_Pcenter_1.In_b_Start      AND
      FB_Pstart_Pend_Pcenter_1.Out_b_Done      AND
      NOT FB_Pstart_Pend_Pcenter_1.Out_b_Error
      THEN
        ui_loop:=33200;
    END_IF
    IF  FB_Pstart_Pend_Pcenter_1.In_b_Start      AND
      FB_Pstart_Pend_Pcenter_1.Out_b_Done      AND
      FB_Pstart_Pend_Pcenter_1.Out_b_Error
      THEN
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        Out_b_Busy R= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Pstart_Pend_Pcenter';
        Out_str_Error:=FB_Pstart_Pend_Pcenter_1.Out_str;
        ui_loop:=0;
    END_IF
  33200://圆弧插补起点 圆心 终点模式:输出计算结果
    FB_Pstart_Pend_Pcenter_1.In_b_Start R= TRUE;
    lr_Arc_Length_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.Out_lr_Arc_Length;
    lr_Arc_Radian_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.Out_lr_Radian;
    b_Arc_Dir_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.In_b_Dir;
    lr_Center_X[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.In_lr_Center_X;
    lr_Center_Y[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.In_lr_Center_Y;
    lr_End_X[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.In_lr_End_X;
    lr_End_Y[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.In_lr_End_Y;
    lr_Arc_Radius_XY[ui_GlueStep_Count]:=FB_Pstart_Pend_Pcenter_1.Out_lr_Radius;
    ui_loop:=40000;
  40000://圆弧插补:计算弧线的长度
    Out_lr_Length[ui_GlueStep_Count]:=SQRT(lr_Arc_Length_XY[ui_GlueStep_Count]*lr_Arc_Length_XY[ui_GlueStep_Count]+
                        In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos*In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos
                        );  
    //判定弧线的长度是否大于0，若非大于0则不执行
    IF  Out_lr_Length[ui_GlueStep_Count]>0
      THEN
        ui_loop:=40100;
      ELSE
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        Out_b_Busy R= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Glue_Dispensing_Trajectory';
        Out_str_Error:='02:The length of this step is 0';
        ui_loop:=0;
    END_IF
  40100://圆弧插补:根据速率 和 长度 计算所需要的点数
    IF  In_dut_GluePro_Step[ui_GlueStep_Count].lr_Speed_Ratio<>0
      THEN
        Out_ui_CamPointNum[ui_GlueStep_Count] := LREAL_TO_UINT(lr_mm_CamPoint * (Out_lr_Length[ui_GlueStep_Count]/1000) * (1/(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Speed_Ratio/1000)));
        ui_Total_CamPoint_Num[ui_GlueStep_Count]:=ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+Out_ui_CamPointNum[ui_GlueStep_Count];
        ui_loop:=40150;
      ELSE
        Out_b_Done S= TRUE;
        Out_b_Error S= TRUE;
        Out_b_Busy R= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Glue_Dispensing_Trajectory';
        Out_str_Error:='03:The speed of this step is 0';
        ui_loop:=0;
    END_IF
  40150://判定当前步数有无超过设定上限
    IF  ui_Total_CamPoint_Num[ui_GlueStep_Count]>(udi_CamPoint_UpLimit-100)
      THEN
        Out_b_Done S= TRUE;
        Out_b_Busy R= TRUE;
        Out_b_Error S= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Glue_Dispensing_Trajectory';
        Out_str_Error:='01:The number of points exceeds 32767';
        ui_loop:=0;
      ELSE
        ui_Loop:=40200;  
    END_IF    
  40200://判定起点在以圆心为座标系原点的时候 是处于哪个象位
    //起点在第一象限
    IF  0>=lr_Center_X[ui_GlueStep_Count]    AND
      0>=lr_Center_Y[ui_GlueStep_Count]
      THEN
        ui_loop:=41000;
    END_IF
    //起点在第二象限
    IF  0<=lr_Center_X[ui_GlueStep_Count]    AND
      0>=lr_Center_Y[ui_GlueStep_Count]
      THEN
        ui_loop:=42000;
    END_IF
    //起点在第三象限
    IF  0<=lr_Center_X[ui_GlueStep_Count]    AND
      0<=lr_Center_Y[ui_GlueStep_Count]
      THEN
        ui_loop:=43000;
    END_IF
    //起点在第四象限
    IF  0>=lr_Center_X[ui_GlueStep_Count]    AND
      0<=lr_Center_Y[ui_GlueStep_Count]
      THEN
        ui_loop:=44000;
    END_IF
  41000://起点落在第一象限
    //以圆心为座标原点，进行坐标的变换
    lr_Start_X_OriginisCenter:=0-lr_Center_X[ui_GlueStep_Count];
    lr_Start_Y_OriginisCenter:=0-lr_Center_Y[ui_GlueStep_Count];
    ui_loop:=41020;
  41020://计算起点和X正坐标轴的角度
    lr_Radian_Ps_XP_OriginisCenter:=ASIN(lr_Start_Y_OriginisCenter/lr_Arc_Radius_XY[ui_GlueStep_Count]);
    ui_loop:=41040;
  41040://判定是顺时针还是逆时针
    IF  b_Arc_Dir_XY[ui_GlueStep_Count]
      THEN
        ui_loop:=41100;
      ELSE
        ui_loop:=41300;
    END_IF
  41100://顺时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_XP_OriginisCenter-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_XP_OriginisCenter-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  41300://逆时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_XP_OriginisCenter+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_XP_OriginisCenter+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  42000://起点落在第二象限
    //以圆心为座标原点，进行坐标的变换
    lr_Start_X_OriginisCenter:=0-lr_Center_X[ui_GlueStep_Count];
    lr_Start_Y_OriginisCenter:=0-lr_Center_Y[ui_GlueStep_Count];
    ui_loop:=42020;
  42020://计算起点和X正坐标轴的角度
    lr_Radian_Ps_YP_OriginisCenter:=ASIN((-lr_Start_X_OriginisCenter)/lr_Arc_Radius_XY[ui_GlueStep_Count]);
    ui_loop:=42040;
  42040://判定是顺时针还是逆时针
    IF  b_Arc_Dir_XY[ui_GlueStep_Count]
      THEN
        ui_loop:=42100;
      ELSE
        ui_loop:=42300;
    END_IF
  42100://顺时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_YP_OriginisCenter+0.5*lr_Pi-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_YP_OriginisCenter+0.5*lr_Pi-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  42300://逆时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_YP_OriginisCenter+0.5*lr_Pi+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_YP_OriginisCenter+0.5*lr_Pi+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  43000://起点落在第三象限
    //以圆心为座标原点，进行坐标的变换
    lr_Start_X_OriginisCenter:=0-lr_Center_X[ui_GlueStep_Count];
    lr_Start_Y_OriginisCenter:=0-lr_Center_Y[ui_GlueStep_Count];
    ui_loop:=43020;
  43020://计算起点和X负坐标轴的角度
    lr_Radian_Ps_XN_OriginisCenter:=ASIN((-lr_Start_Y_OriginisCenter)/lr_Arc_Radius_XY[ui_GlueStep_Count]);
    ui_loop:=43040;
  43040://判定是顺时针还是逆时针
    IF  b_Arc_Dir_XY[ui_GlueStep_Count]
      THEN
        ui_loop:=43100;
      ELSE
        ui_loop:=43300;
    END_IF
  43100://顺时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_XN_OriginisCenter+lr_Pi-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_XN_OriginisCenter+lr_Pi-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  43300://逆时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_XN_OriginisCenter+lr_Pi+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_XN_OriginisCenter+lr_Pi+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  44000://起点落在第四象限
    //以圆心为座标原点，进行坐标的变换
    lr_Start_X_OriginisCenter:=0-lr_Center_X[ui_GlueStep_Count];
    lr_Start_Y_OriginisCenter:=0-lr_Center_Y[ui_GlueStep_Count];
    ui_loop:=44020;
  44020://计算起点和X负坐标轴的角度
    lr_Radian_Ps_YN_OriginisCenter:=ASIN(lr_Start_X_OriginisCenter/lr_Arc_Radius_XY[ui_GlueStep_Count]);
    ui_loop:=44040;
  44040://判定是顺时针还是逆时针
    IF  b_Arc_Dir_XY[ui_GlueStep_Count]
      THEN
        ui_loop:=44100;
      ELSE
        ui_loop:=44300;
    END_IF
  44100://顺时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_YN_OriginisCenter+1.5*lr_Pi-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_YN_OriginisCenter+1.5*lr_Pi-(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  44300://逆时针
    FOR ui_CamPoint_Index:=(ui_Total_CamPoint_Num_After[ui_GlueStep_Count]) TO (ui_Total_CamPoint_Num[ui_GlueStep_Count]-1) BY 1 DO
      Out_lr_X[ui_CamPoint_Index]:=lr_PrePoint_X[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*COS(lr_Radian_Ps_YN_OriginisCenter+1.5*lr_Pi+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_X[ui_GlueStep_Count];
      Out_lr_Y[ui_CamPoint_Index]:=lr_PrePoint_Y[ui_GlueStep_Count]+lr_Arc_Radius_XY[ui_GlueStep_Count]*SIN(lr_Radian_Ps_YN_OriginisCenter+1.5*lr_Pi+(lr_Arc_Radian_XY[ui_GlueStep_Count]/Out_ui_CamPointNum[ui_GlueStep_Count])*ui_For_i)+lr_Center_Y[ui_GlueStep_Count];
      Out_lr_Z[ui_CamPoint_Index]:=lr_PrePoint_Z[ui_GlueStep_Count]+ui_For_i*(In_dut_GluePro_Step[ui_GlueStep_Count].lr_Z_EndPos/Out_ui_CamPointNum[ui_GlueStep_Count]);
      ui_For_i:=ui_For_i+1;
    END_FOR
    ui_loop:=50000;
  50000://圆弧插补:将当前步数的点数加入ui_Total_CamPoint_Num_After中，让下一步点胶步数可用
    ui_Total_CamPoint_Num_After[ui_GlueStep_Count+1]:=ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+Out_ui_CamPointNum[ui_GlueStep_Count];
    ui_loop:=50100;
  50100://直线插补:存入下一个点胶步数的X Y Z坐标
    lr_PrePoint_X[ui_GlueStep_Count+1]:=Out_lr_X[ui_Total_CamPoint_Num[ui_GlueStep_Count]-1];
    lr_PrePoint_Y[ui_GlueStep_Count+1]:=Out_lr_Y[ui_Total_CamPoint_Num[ui_GlueStep_Count]-1];
    lr_PrePoint_Z[ui_GlueStep_Count+1]:=Out_lr_Z[ui_Total_CamPoint_Num[ui_GlueStep_Count]-1];
    Out_udi_Total:=ui_Total_CamPoint_Num[ui_GlueStep_Count];
    ui_loop:=51000;
  51000://判定延时开胶的位置是否是0
    IF  In_dut_GluePro_Step[ui_GlueStep_Count].lr_Delay_OpenGlue>0
      THEN//计算延时开胶的位置是否大于线路的长度
        IF  In_dut_GluePro_Step[ui_GlueStep_Count].lr_Delay_OpenGlue>Out_lr_Length[ui_GlueStep_Count]
          THEN
            In_dut_GluePro_Step[ui_GlueStep_Count].lr_Delay_OpenGlue:=Out_lr_Length[ui_GlueStep_Count];
            ui_loop:=51100;
          ELSE
            ui_loop:=51100;
        END_IF
      ELSE
        ui_Step_Start_GluePoint[ui_GlueStep_Count]:=0;
        Out_ui_Total_Start_GluePoint[ui_GlueStep_Count]:=ui_Total_CamPoint_Num_After[ui_GlueStep_Count];
        ui_loop:=51500;
    END_IF
  51100://计算延时开胶的点数
    ui_Step_Start_GluePoint[ui_GlueStep_Count]:=LREAL_TO_UINT((In_dut_GluePro_Step[ui_GlueStep_Count].lr_Delay_OpenGlue/(Out_lr_Length[ui_GlueStep_Count]/1000))*(Out_ui_CamPointNum[ui_GlueStep_Count]/1000));
    Out_ui_Total_Start_GluePoint[ui_GlueStep_Count]:=ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+ui_Step_Start_GluePoint[ui_GlueStep_Count];
    ui_loop:=51500;
  51500://判定提前关胶的位置是否是0
    IF  In_dut_GluePro_Step[ui_GlueStep_Count].lr_Ahead_CloseGlue>0
      THEN//判定提前关胶的位置是否大于线路的长度
        IF  In_dut_GluePro_Step[ui_GlueStep_Count].lr_Ahead_CloseGlue>Out_lr_Length[ui_GlueStep_Count]
          THEN
            In_dut_GluePro_Step[ui_GlueStep_Count].lr_Ahead_CloseGlue:=Out_lr_Length[ui_GlueStep_Count];
            ui_loop:=51600;
          ELSE
            ui_loop:=51600;
        END_IF
      ELSE
        ui_Step_End_GluePoint[ui_GlueStep_Count]:=Out_ui_CamPointNum[ui_GlueStep_Count];
        Out_ui_Total_End_GluePoint[ui_GlueStep_Count]:=ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+Out_ui_CamPointNum[ui_GlueStep_Count];
        ui_loop:=52000;
    END_IF
  51600://计算提前关胶的点数
    ui_Step_End_GluePoint[ui_GlueStep_Count]:=LREAL_TO_UINT(((Out_lr_Length[ui_GlueStep_Count]-In_dut_GluePro_Step[ui_GlueStep_Count].lr_Ahead_CloseGlue)/Out_lr_Length[ui_GlueStep_Count])*Out_ui_CamPointNum[ui_GlueStep_Count]);
    Out_ui_Total_End_GluePoint[ui_GlueStep_Count]:=ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+ui_Step_End_GluePoint[ui_GlueStep_Count];
    ui_loop:=52000;
  52000://直线插补:点胶步数＋1
    ui_GlueStep_Count:=ui_GlueStep_Count+1;
    ui_loop:=52100;
  52100://判定当前点数是否超过设定的上限
    IF  Out_udi_Total>(udi_CamPoint_UpLimit-100)
      THEN
        Out_b_Busy R= TRUE;
        Out_b_Error S= TRUE;
        Out_ui_Error_Step:=ui_GlueStep_Count;
        Out_str_Error_FB:='FB_Glue_Dispensing_Trajectory';
        Out_str_Error:='01:The number of points exceeds 32767';
        ui_loop:=60000;
      ELSE
        ui_Loop:=1000;  
    END_IF
  60000:
    FOR ui_For_i:=0 TO 50 BY 1 DO
      Out_lr_X[ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+ui_For_i]:=Out_lr_X[Out_udi_Total-1];
      Out_lr_Y[ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+ui_For_i]:=Out_lr_Y[Out_udi_Total-1];
      Out_lr_Z[ui_Total_CamPoint_Num_After[ui_GlueStep_Count]+ui_For_i]:=Out_lr_Z[Out_udi_Total-1];
    END_FOR
    ui_Loop:=60100;  
  60100://计算结束
    Out_b_Busy R= TRUE;
    Out_b_Done S= TRUE;
    ui_Loop:=0;
END_CASE

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！