摘要：以前用凸轮的时候，只是简单算出所需路径，没给操机人员开发一个接口来做自定义编程。前段日子设计并编程了一台同样需要点胶的设备，选型时看中了基恩士 KV8000 的点参数功能，这接口向 PLC 编程人员开放，能定义插补运动的路径。后来还发现点参数的编辑 KV-XH
一，前言
以前用凸轮的时候，只是简单算出所需路径，没给操机人员开发一个接口来做自定义编程。前段日子设计并编程了一台同样需要点胶的设备，选型时看中了基恩士 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, ±√{RR - [L/2][L/2]})；
第六，经过平次坐标后的坐标是：([L/2]cosθ - (±√{RR - [L/2]*[L/2]})sinθ, [L/2]sinθ + (±√{RR - [L/2][L/2]})cosθ)；
第七，从以 A 为坐标原点变换到实际坐标后，圆心的坐标是：([L/2]cosθ - (±√{RR - [L/2][L/2]})sinθ + XA, [L/2]sinθ + (±√{RR - [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.已知起点，终点，圆心，半径，弧度——求插补座标
构建三轴圆弧插补的思路
得先弄清楚三轴圆弧线的长度。单条圆弧运动可以想象成是圆柱体上的一条圆弧线 LXYZ ，高度是 Z 轴方向的高度差 H ，长度的起点是 (X1,Y1) ，终点是 (X2,Y2) 。圆柱体在 Z 方向的投影上有个圆，上面的一段圆弧是 LXY ，要是能知道圆弧 LXY 的长度 L ，就能算出圆柱体上 LXYZ 的长度。得知道起点 (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| = √((X1 - X3)(X1 - X3) + (Y1 - Y3)(Y1 - Y3)) ；向量 CB 同理，从 C 到 B ，向量 CB 表示为 CB = [X2 - X3,Y1 - Y3] ，然后求它的模 |CB| = √((X2 - X3)(X2 - X3) + (Y2 - Y3)(Y2 - Y3)) ；最后求这两个向量的数量积，CACB = ((X1 - X3)(X2 - X3) + (Y1 - Y3)(Y2 - Y3)) ；由公式 cos(θ) = （CACB)/(|CA|*|CB|) ；最后通过反余弦函数得出θ的数值。另外说一句：还有一种算法是用反正切函数，不过我自己验证过，没弄明白。附上链接地址：已知圆上两点坐标求角度 - 百度经验(https://jingyan.baidu.com/article/4ae03de3da4fc97ffe9e6b2c.html) 。三，代码
1.已知起点，终点，半径的代码
//已知起点 终点 半径 圆弧方向 劣弧优弧 求圆心 求弧度 求弧长FUNCTION_BLOCK FB_Pstart_Pend_RadiusVAR_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_VARVAR_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_VARVAR 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_VARVAR CONSTANT lr_Pi: LREAL := 3.1415926535897931;// PiEND_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 zeroCASE 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=(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_BO_P=(In_lr_Radius-0.0001) AND lr_BO_N0 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_CEIn_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_YIn_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_XIn_lr_End_X AND In_lr_Start_YIn_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_XIn_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(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(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_CASE2.已知起点，终点，中间点的代码
//已知三点坐标 求圆心 半径 弧度 弧长 圆弧方向//仅适用于三点都不相同FUNCTION_BLOCK FB_Pstart_Pend_PmiddleVAR_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_VARVAR_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_VARVAR 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_VARVAR CONSTANT lr_Pi: LREAL := 3.1415926535897931;// PiEND_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 zeroCASE 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_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_XIn_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_YIn_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_XIn_lr_End_X AND In_lr_Start_YIn_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_XIn_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(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(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_CASE3.已知起点，终点，圆心的代码
FUNCTION_BLOCK FB_Pstart_Pend_PcenterVAR_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_VARVAR_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_VARVAR 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_VARVAR CONSTANT lr_Pi: LREAL := 3.1415926535897931;// PiEND_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 zeroCASE 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_LSO0 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=-0.0001 AND lr_Radian_CS_CEIn_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_YIn_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_XIn_lr_End_X AND In_lr_Start_YIn_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_XIn_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(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(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_CASE4.点胶步序结构体变量
//步序程序的结构体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_STRUCTEND_TYPE5.插补功能块
FUNCTION_BLOCK FB_Glue_Dispensing_TrajectoryVAR_INPUT In_b_Start : BOOL ;//开始计算 In_ui_GlueStep_Num : UINT ;//点胶步数 In_dut_GluePro_Step : ARRAY[0..19] OF dut_GluePro_Step ;//点胶程序的设定 END_VARVAR_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_VARVAR 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_VARVAR CONSTANT lr_mm_CamPoint : LREAL := 100 ;//1mm需要多少凸轮点数 lr_Pi : LREAL := 3.1415926535897931 ;// Pi udi_CamPoint_UpLimit : UDINT := 32767 ;//凸轮END_VARtrig_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_Ratio0 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_Ratio0 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_Y[ui_GlueStep_Count] THEN ui_loop:=42000; END_IF //起点在第三象限 IF 0=lr_Center_X[ui_GlueStep_Count] AND 00 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来源：PLC技术玩一点号
免责声明：本站系转载，并不代表本网赞同其观点和对其真实性负责。如涉及作品内容、版权和其它问题，请在30日内与本站联系，我们将在第一时间删除内容!