WPF上位机+Codesys的CNC

最新推荐文章于 2024-05-21 15:34:50 发布

AlongWu1122

最新推荐文章于 2024-05-21 15:34:50 发布

阅读量1.1w

点赞数 26

分类专栏： codesys WPF CNC 文章标签：制造物联网 wpf c# Powered by 金山文档

本文链接：https://blog.csdn.net/weixin_42386991/article/details/128552900

版权

codesys 同时被 3 个专栏收录

10 篇文章 22 订阅

订阅专栏

CNC

2 篇文章 1 订阅

订阅专栏

WPF

1 篇文章 0 订阅

订阅专栏

一、概述

利用C#的WPF做上位机，通过Udp与Codesys通信并利用Codesys的File和Cnc模块，传输CNC的G代码文件和执行G代码。本程序展示了，Codesys的Cnc解析和执行G代码的完整过程、Txt读写文件操作过程以及Udp通信交互过程。

二、程序架构

2.1 WPF程序架构

2.2Codesys程序架构

三、代码展示

3.1WPF的代码展示

3.1.1MainCommand的代码

//作者:AlongWu

using CncTest.Common;
using Microsoft.Win32;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;

namespace CncTest.Model
{
    public class MainCommand : NotifyBase
    {
        /* Cmmand 变量          */
        public CommandBase InputCommand { get; set; }
        public CommandBase StartCommand { get; set; }
        public CommandBase ResetCommand { get; set; }
        public CommandBase StopCommand { get; set; }

        /* 基础 变量            */
        private MainWinModel MainWModel { get; set; }
        private delegate void UpdateUIDelegate();
        private ItemsControl CncCodeList;




        /* udp命令             */

        private byte[] GetRunningInfo = { 0x05, 0x00, 0x01, 0xa0, 0x01 };
        private byte[] SetCncStart = { 0x05, 0x00, 0x02, 0xE0, 0x00 };
        private byte[] StopCnc = { 0x05, 0x00, 0x03, 0x21, 0xc0 };
        private byte[] ResetCnc = { 0x05, 0x00, 0x04, 0x60, 0x02 };
        private byte[] GetPlcCncCode = { 0x05, 0x00, 0x14, 0x61, 0xCe };
        private byte[] ReadyToSendCncCode= { 0x05, 0x00, 0x0a, 0xe1, 0xC6 };

        /* udp通信             */
        private bool IsEnable;
        private IPEndPoint epServer;
        private UdpClient local;
        private string host;
        private short port = 10001;

        private bool txtRecei;
        private bool IsUdpcRecvStart = false;
        IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 10001);
        int UdpsendCount;
        private int FileReceiLostCount;
        private byte[] tmp4byte = new byte[4];
        private byte[] tmp8byte = new byte[8];
        private ushort crc, polynom, polynom2, crc2;
        private bool CmdSendContinue;


        /*  cnc 变量              */
        private Int16 CncCodeRowId;
        private UInt16 CncState,Mcode;
        private byte FileSendId;
     
       
    
        public MainCommand(MainWinModel mm,ItemsControl codeList)
        {

            MainWModel = mm;
            CommandInit();
            CncCodeList = codeList;
            ConnectPlc();
        }
        /* Common function start */


        /// <summary>
        /// CommandInit Comand指令初始化
        /// </summary>
        private void CommandInit()
        {
            this.InputCommand = new CommandBase();
            this.InputCommand.DoCanExecute = CommandDoCanFunc;
            this.InputCommand.DoExecute = InputAct;

            this.StartCommand = new CommandBase();
            this.StartCommand.DoCanExecute = CommandDoCanFunc;
            this.StartCommand.DoExecute = StartAct;

            this.ResetCommand = new CommandBase();
            this.ResetCommand.DoCanExecute = CommandDoCanFunc;
            this.ResetCommand.DoExecute = ResetAct;

            this.StopCommand = new CommandBase();
            this.StopCommand.DoCanExecute = CommandDoCanFunc;
            this.StopCommand.DoExecute = StopAct;         
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="o"></param>
        /// <returns></returns>
        private bool CommandDoCanFunc(Object o)
        {
            return true;
        }
        /* Common function end */

        /* Command Acts start */

    

        /// <summary>
        /// 导入按钮的Act
        /// 利用Task，新建线程去执行带有延时的动作
        /// 1,打开选取的文件。
        /// 2,通过UDP发送给PLC。
        /// 3,读取最新的cnc代码并且更新代码列表。
        /// </summary>
        /// <param name="o"></param>
        private void InputAct(object o)
        {
            OpenFileDialog openFile = new OpenFileDialog();
            string[] TxtCodeStr;          
            CncCode cc;
            int i, readoffset, n;
            byte[] byData = new byte[1024];                 
            CancellationTokenSource TokenSource;
            TokenSource = new CancellationTokenSource();
            Task Task;
            FileStream file;
            int size;
           
            //创建byte数组
            byte[] bytes = null;
            byte[] tmp2;
            int addr = 0;
            
            openFile.Filter = "TXT(*.txt)|*.txt";

            if (openFile.ShowDialog() == true)
            {              
                    Task = new Task(async () =>
                    {

                        //通知PLC,"启动接收cnc代码"
                        UdpCmd(ReadyToSendCncCode, ReadyToSendCncCode.Length);                 
                        await Task.Delay(200);
                    
                        FileSendId = 0;
                        try
                        {
                            file = new FileStream(openFile.FileName, FileMode.Open);//打开txt文件
                            file.Seek(0, SeekOrigin.Begin);
                            readoffset = 0;

                            //用while循环，每1000个byte发送一次。
                            do
                            {                             
                                file.Seek(readoffset, SeekOrigin.Begin);            //文件定位
                                n = file.Read(byData, 0, 1000);                     //读取txt文件内容
                                if (n <= 0)
                                {
                                    break;
                                }                          
                                size = n + 6;
                                bytes = new byte[size];
                                addr = 0;
                                tmp2 = BitConverter.GetBytes((Int16)size);
                                Buffer.BlockCopy(tmp2, 0, bytes, addr, 2);
                                addr = addr + 2;
                                bytes[2] = 11;                                      //命令码11
                                bytes[3] = FileSendId;                              //发送的序号                              
                                Buffer.BlockCopy(byData, 0, bytes, 4, n);           //cnc文件的字节复制到数据帧                             
                                CalcCRC16_Modbus(ref bytes);                        //生成crc校验                                                  
                                UdpCmd(bytes, bytes.Length);                        //通过udp发出数据
                                await Task.Delay(100);
                                FileSendId++;                                       //发送的序号 +1
                                if (n < 1000)
                                {
                                    //如果本次发送的字节小于1000，即最后一帧，退出while。
                                    break;
                                }
                                readoffset = readoffset + n;                        //文件读取位后移
                            } while (IsUdpcRecvStart);
                            

                            file.Close();                                           //关闭txt文件
                            }catch (IOException e)
                                {
                                    Console.WriteLine(e.ToString());
                                }

                        //发送完成后，再次读取plc的cnc代码并展示

                        file = new FileStream("cnc.txt", FileMode.OpenOrCreate);    //打开固定位置的txt文件
                        file.Seek(0, SeekOrigin.Begin);
                        file.SetLength(0);
                        file.Close();

                        //获取PLC更新后的CNC文件数据
                        txtRecei = false;
                        UdpCmd(GetPlcCncCode, GetPlcCncCode.Length);                //udp发出读取PLC的cnc代码指令                
                        FileReceiLostCount = 0;
                        while (!txtRecei)
                        {
                            await Task.Delay(50);
                            FileReceiLostCount++;                         
                            if (FileReceiLostCount > 20)
                            {
                                MessageBox.Show("读取PLC的CNC文件失败");
                                return;
                            }
                        }

                        //更新 View的CodeList
                        ObservableCollection<CncCode> CList;
                        TxtCodeStr = File.ReadAllLines("cnc.txt", Encoding.UTF8);
                        CList = new ObservableCollection<CncCode>();
                         n = TxtCodeStr.Length;
                        if (n > 0)
                        {
                            CList.Clear();
                            for (i = 0; i < n; i++)
                            {
                                cc = new CncCode();
                                cc.Code = TxtCodeStr[i];
                                cc.BgColor = "White";
                                CList.Add(cc);
                            }
                        }
                        MainWModel.CodeList = CList;

                    }, TokenSource.Token);
                    Task.Start();

            }

            
   

        }

        /// <summary>
        /// Cnc加工按钮的Act
        /// </summary>
        /// <param name="o"></param>
        private void StartAct(object o)
        {
            UdpCmd(SetCncStart, SetCncStart.Length);
        }

        /// <summary>
        /// Cnc复位按钮的Act
        /// </summary>
        /// <param name="o"></param>
        private void ResetAct(object o)
        {
            int n, i;
            UdpCmd(ResetCnc, ResetCnc.Length);
   
            n = MainWModel.CodeList.Count;
            for (i=0;i<n;i++)
            {
              
                MainWModel.CodeList[i].BgColor = "White";              
            }
            CncCodeList.Items.Refresh();
        }

        /// <summary>
        /// Cnc停止按钮的Act
        /// </summary>
        /// <param name="o"></param>
        private void StopAct(object o)
        {
            UdpCmd(StopCnc, StopCnc.Length);
        }

        /* Command Acts end */
        /* Function start  */

        /// <summary>
        /// 连接PLC的函数
        /// 1，绑定Udp端口和ip。
        /// 2，访问plc获取当前的cnc代码。
        /// 3，启动循环查询plc当前状态。
        /// </summary>
        private void ConnectPlc()
        {

            CancellationTokenSource TokenSource;
            TokenSource = new CancellationTokenSource();
            Task Task;
            FileStream file;
            string[] TxtCodeStr;
            CncCode cc;
            int i,n;

            //打开udp端口并绑定plc的ip和port
            UdpBlind("192.168.0.200",10001);

            Task = new Task(async () =>
            {

                file = new FileStream("cnc.txt", FileMode.OpenOrCreate);//打开txt文件
                file.Seek(0, SeekOrigin.Begin);
                file.SetLength(0);
                file.Close();

                //获取PLC更新后的CNC文件数据
                txtRecei = false;
                CmdSendContinue = true;
                UdpCmd(GetPlcCncCode, GetPlcCncCode.Length);            //获取plc的cnc代码的命令
                FileReceiLostCount = 0;
                while (!txtRecei)
                {
                    await Task.Delay(50);
                    FileReceiLostCount++;
                    if (FileReceiLostCount > 20)
                    {
                        MessageBox.Show("读取PLC的CNC文件失败");
                        return;
                    }
                }

                //更新 View的CodeList
                ObservableCollection<CncCode> CList;
                TxtCodeStr = File.ReadAllLines("cnc.txt", Encoding.UTF8);
                CList = new ObservableCollection<CncCode>();
                n = TxtCodeStr.Length;
                if (n > 0)
                {
                    CList.Clear();
                    for (i = 0; i < n; i++)
                    {
                        cc = new CncCode();
                        cc.Code = TxtCodeStr[i];
                        cc.BgColor = "White";
                        CList.Add(cc);
                    }
                }
                MainWModel.CodeList = CList;

                //开启循环查询plc的状态
                while (IsUdpcRecvStart)
                {
                    CmdSendContinue = false;
                    UdpSend(GetRunningInfo, GetRunningInfo.Length);
                    await Task.Delay(50);
                    CmdSendContinue = true;
                    await Task.Delay(50);
                }      
            }, TokenSource.Token);
            Task.Start();
        }



        /* Function end  */

        /* udp function start  */


        /// <summary>
        /// 绑定Udp的ip和port
        /// </summary>
        /// <param name="Targethost"></param>
        /// <param name="ServerPort"></param>
        public void UdpBlind(string Targethost, short ServerPort)
        {

            if (local == null)
            {
                host = Targethost;
                port = ServerPort;
                epServer = new IPEndPoint(IPAddress.Parse(host), port);
                local = new UdpClient(10000);    //绑定本机IP和端口10000
                                                 //开始异步接收启动一个线程;该线程启动函数是ReceiveCallback该函数中结束挂起的异步接收
                                                 //    local.BeginReceive(new AsyncCallback(UdpAsynReceiveCallback), null);
            }
            IsEnable = true;
            IsUdpcRecvStart = true;
            local.BeginReceive(new AsyncCallback(UdpAsynReceiveCallback), null);
            Console.WriteLine("Udp = " + host + ":" + port.ToString() + " Init.");

        }
        /// <summary>
        /// 关闭Udp通信
        /// </summary>
        public void UdpStop()
        {
            IsUdpcRecvStart = false;
            Console.WriteLine("Udp End.");
        }
        /// <summary>
        /// 获取Upd连接状态
        /// </summary>
        /// <returns></returns>
        public bool IsConnect()
        {
            return IsUdpcRecvStart;
        }
        /// <summary>
        /// Udp发送函数
        /// </summary>
        /// <param name="msg"></param>
        /// <param name="len"></param>
        public void UdpSend(byte[] msg, int len)
        {
            local.BeginSend(msg, len, epServer, new AsyncCallback(UdpAsynSendCallback), null);
        }
        /// <summary>
        /// Udp控制指令发送
        /// </summary>
        /// <param name="msg"></param>
        /// <param name="len"></param>
        public void UdpCmd(byte[] msg, int len)
        {
            while (!CmdSendContinue)
            {
                //等待周期访问plc的指令空闲
            }
            local.BeginSend(msg, len, epServer, new AsyncCallback(UdpAsynSendCallback), null);
        }


        /// <summary>
        /// Udp异步发送回调函数
        /// </summary>
        /// <param name="iar"></param>
        private void UdpAsynSendCallback(IAsyncResult iar)
        {
            UdpsendCount = local.EndSend(iar);
            if (UdpsendCount == 0)
            {
                Console.WriteLine("Udp 发送失败");
            }
        }


        /// <summary>
        /// Udp异步接收回调函数
        /// </summary>
        /// <param name="iar"></param>
        private void UdpAsynReceiveCallback(IAsyncResult iar)
        {
            byte[] buff;
            try
            {
                if (local.Client != null && IsEnable)
                {
                    buff = local.EndReceive(iar, ref RemoteIpEndPoint);
                    //调用DecodeRecei处理接收字节
                    DecodeRecei(buff);
                    if (IsUdpcRecvStart)
                    {
                        //再次开启异步接收
                        local.BeginReceive(new AsyncCallback(UdpAsynReceiveCallback), null);
                    }
                }
                else
                {
                    Console.WriteLine("接收未启动或正在关闭中");
                }
            }
            catch
            {
                Console.WriteLine("接收过程发生错误，再次启动异步接收。");
                if (IsUdpcRecvStart)
                {
                    local.BeginReceive(new AsyncCallback(UdpAsynReceiveCallback), null);
                }
            }


        }


        /// <summary>
        /// 断开Udp连接
        /// </summary>
        public void Disconnect()
        {
            IsEnable = false;
            IsUdpcRecvStart = false;
            Thread.Sleep(200);
            local.Close();

        }



        /* common function start  */

        /// <summary>
        /// 解析Udp接收的字节
        /// </summary>
        /// <param name="buff"></param>
        private void DecodeRecei(byte[] buff)
        {
            int addr = 0;   
            int FrameSize;
            FileStream fs;
            byte[] tmp;
            if (CheckCRC16_Modbus(buff))
            {
                FrameSize = bin2Short(buff[0], buff[1]);
                switch (buff[2])
                {
                    case 1:
                        //命令码1，获取plc状态值
                        addr = 3;
                        MainWModel.X = bin2fl32(buff[addr], buff[addr + 1], buff[addr + 2], buff[addr + 3]);
                        addr = addr + 4;
                        MainWModel.Y = bin2fl32(buff[addr], buff[addr + 1], buff[addr + 2], buff[addr + 3]);
                        addr = addr + 4;
                        MainWModel.Z = bin2fl32(buff[addr], buff[addr + 1], buff[addr + 2], buff[addr + 3]);
                        addr = addr + 4;
                        CncCodeRowId = bin2Short(buff[addr], buff[addr + 1]);
                        addr = addr + 2;
                        Mcode = bin2UShort(buff[addr], buff[addr + 1]);
                        addr = addr + 2;                
                        CncState = bin2UShort(buff[addr], buff[addr + 1]);
                        //委托的方式，更新cnc代码list
                        ThreadCncCodeListUI();
                        break;

                    case 2:
                        //启动cnc
                        // nothing                      
                        break;

                    case 3:
                        //停止cnc
                        // nothing
                        break;
                    case 4:
                        //重置cnc
                        // nothing
                        break;
                                   
                    case 10:
                        //准备PC发送cnc文件
                        // nothing
                        break;
                    case 11:
                        //PC发送cnc文件字节流
                        // nothing
                        break;
                    
                    case 20:
                        //读取cnc文件
                        //nothing        
                        break;
                    case 21:
                        //PLC发送的cnc文件字节流                   
                        fs = new FileStream("cnc.txt", FileMode.Append, FileAccess.Write);
                        //获取字节数组
                        tmp = buff.Skip(3).Take(FrameSize - 5).ToArray();
                        //开始写入
                        fs.Write(tmp, 0, FrameSize - 5);
                        //清空缓冲区，关闭流
                        fs.Flush();
                        fs.Close();
                        FileReceiLostCount = 0;
                        
                        //固定每次1000字节，小于1000字节即为最后1帧
                        if ((FrameSize - 5) < 1000)
                        {
                            txtRecei = true; 
                        }
                        break;
  
                    default:
                        //nothing
                        break;
                }
            }
        }

        /// <summary>
        /// CRC16-Modbus校验
        /// </summary>
        /// <param name="pDataBytes"></param>
        /// <returns>校验成功/失败</returns>
        private bool CheckCRC16_Modbus(byte[] pDataBytes)
        {
            crc = 0xffff;
            polynom = 0xA001;    // modbus 
            int i, j;
            if (pDataBytes.Length > 2)
            {
                for (i = 0; i < pDataBytes.Length - 2; i++)
                {
                    crc ^= pDataBytes[i];
                    for (j = 0; j < 8; j++)
                    {
                        if ((crc & 0x01) == 0x01)
                        {
                            crc >>= 1;
                            crc ^= polynom;
                        }
                        else
                        {
                            crc >>= 1;
                        }
                    }
                }
                byte[] bs = new byte[2];

                bs[0] = (byte)crc;
                bs[1] = (byte)(crc >> 8);

                if (bs[0] == pDataBytes[pDataBytes.Length - 2] && bs[1] == pDataBytes[pDataBytes.Length - 1])
                {
                    return true;
                }
                else
                {
                    return false;
                }

            }
            else
            {
                return false;
            }

        }
        /// <summary>
        /// 生成CRC16-Modbus校验，并把校验码附在字节数组最后2个byte。
        /// </summary>
        /// <param name="pDataBytes"></param>
        public void CalcCRC16_Modbus(ref byte[] pDataBytes)
        {
            crc2 = 0xffff;
            polynom2 = 0xA001;    // modbus 
            int i, j;
            for (i = 0; i < pDataBytes.Length - 2; i++)
            {
                crc2 ^= pDataBytes[i];
                for (j = 0; j < 8; j++)
                {
                    if ((crc2 & 0x01) == 0x01)
                    {
                        crc2 >>= 1;
                        crc2 ^= polynom2;
                    }
                    else
                    {
                        crc2 >>= 1;
                    }
                }
            }
            byte[] bs = new byte[2];

            bs[0] = (byte)crc2;
            bs[1] = (byte)(crc2 >> 8);

            pDataBytes[pDataBytes.Length - 2] = bs[0];
            pDataBytes[pDataBytes.Length - 1] = bs[1];
        }
        /// <summary>
        /// byte转Float函数
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <param name="a3"></param>
        /// <returns></returns>
        public Single bin2fl32(byte a0, byte a1, byte a2, byte a3)
        {
            tmp4byte[0] = a0;
            tmp4byte[1] = a1;
            tmp4byte[2] = a2;
            tmp4byte[3] = a3;
            return BitConverter.ToSingle(tmp4byte, 0);
        }
        /// <summary>
        /// byte转Int32函数
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <param name="a3"></param>
        /// <returns></returns>
        public Int32 bin2int32(byte a0, byte a1, byte a2, byte a3)
        {
            tmp4byte[0] = a0;
            tmp4byte[1] = a1;
            tmp4byte[2] = a2;
            tmp4byte[3] = a3;
            return BitConverter.ToInt32(tmp4byte, 0);
        }
        /// <summary>
        /// byte转Int16函数
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <returns></returns>
        private Int16 bin2Short(byte a0, byte a1)
        {
            tmp4byte[0] = a0;
            tmp4byte[1] = a1;
            return BitConverter.ToInt16(tmp4byte, 0);
        }
        /// <summary>
        /// byte转UInt16函数
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <returns></returns>
        private UInt16 bin2UShort(byte a0, byte a1)
        {
            tmp4byte[0] = a0;
            tmp4byte[1] = a1;
            return BitConverter.ToUInt16(tmp4byte, 0);
        }

        /// <summary>
        /// byte转UInt64函数
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <param name="a3"></param>
        /// <param name="a4"></param>
        /// <param name="a5"></param>
        /// <param name="a6"></param>
        /// <param name="a7"></param>
        /// <returns></returns>
        public UInt64 bin2UInt64(byte a0, byte a1, byte a2, byte a3, byte a4, byte a5, byte a6, byte a7)
        {
            tmp8byte[0] = a0;
            tmp8byte[1] = a1;
            tmp8byte[2] = a2;
            tmp8byte[3] = a3;
            tmp8byte[4] = a4;
            tmp8byte[5] = a5;
            tmp8byte[6] = a6;
            tmp8byte[7] = a7;
            return BitConverter.ToUInt64(tmp8byte, 0);
        }


        /// <summary>
        /// byte转UInt32函数
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        /// <param name="a3"></param>
        /// <returns></returns>
        public UInt32 bin2UInt32(byte a0, byte a1, byte a2, byte a3)
        {
            tmp4byte[0] = a0;
            tmp4byte[1] = a1;
            tmp4byte[2] = a2;
            tmp4byte[3] = a3;
            return BitConverter.ToUInt32(tmp4byte, 0);
        }


        /* udp function end  */

        /// <summary>
        /// 委托方式，更新View的codelist
        /// </summary>
        private void ThreadCncCodeListUI()
        {
            UpdateUIDelegate updateUIDelegate = new UpdateUIDelegate(CncCodeListUI);
            System.Windows.Application.Current.Dispatcher.BeginInvoke(updateUIDelegate);
        }

        /// <summary>
        /// 更新View的codelist
        /// </summary>
        private void CncCodeListUI()
        {
            int n,i;
            n = MainWModel.CodeList.Count;
            if (CncCodeRowId >= 0 && n > 0)
            {
                if (CncCodeRowId > 0)
                {
                    MainWModel.CodeList[CncCodeRowId - 1].BgColor = "White";
                    MainWModel.CodeList[CncCodeRowId].BgColor = "LightGray";
                }
                else
                {
                    MainWModel.CodeList[CncCodeRowId].BgColor = "LightGray";
                }
                // Console.WriteLine("id=" + CncCodeRowId.ToString());
                CncCodeList.Items.Refresh();
            }
        }


    }
}

3.2Codesys的代码展示

3.2.1CncDecode_PRG

//作者:AlongWu

PROGRAM CncDecode_PRG
VAR

ReadNC                                        :SMC_ReadNCFile2;                            //读取并解析cnc代码功能块 实例
NcIpe                                        :SMC_NCInterpreter;                            //cnc代码格式转换器功能块 实例

RoundPath                                    :SMC_RoundPath;                                //路径预处理，圆弧平滑叠加， G52激活，G50取消
SmoothPath                                    :SMC_SmoothPath;                            //路径预处理，B样条平滑叠加，G51激活，G50取消

ToolCorr                                    :SMC_ToolCorr;                                //路径预处理，刀具补偿路径.    G41/G42 激活，G40取消
AvoidLoop                                    :SMC_AvoidLoop;                                //路径预处理，路径交叉避免.    G61激活，G60取消


IpeBuf                                        :ARRAY[0..99] OF SMC_GEOINFO;                //SMC_NCInterpreter缓存空间
RoundPathBuf                                :ARRAY[0..99] OF SMC_GEOINFO;                //SMC_RoundPath缓存空间
SmoothPathBuf                                :ARRAY[0..99] OF SMC_GEOINFO;                //SMC_SmoothPath缓存空间
ToolCorrBuf                                    :ARRAY[0..99] OF SMC_GEOINFO;                //SMC_ToolCorr缓存空间
AvoidLoopBuf                                :ARRAY[0..99] OF SMC_GEOINFO;                //SMC_AvoidLoop缓存空间

CheckVel_F_TRIG                                :F_TRIG;                                    //GVL.CheckVel.bBusy下降沿

END_VAR



//读取并解析cnc代码功能块 实例
ReadNC(
    bExecute:= GVL.B_ReadFile, 
    sFileName:= GVL.STR_CncFilePath, 
    pvl:= , 
    fDefaultVel:= 20, 
    fDefaultAccel:= 20, 
    fDefaultDecel:= 20, 
    fDefaultVelFF:= 20, 
    fDefaultAccelFF:= 20, 
    fDefaultDecelFF:= 20, 
    b3DMode:= TRUE , 
    bStepSuppress:= , 
    aSubProgramDirs:= , 
    bParenthesesAsComments:= , 
    bDisableJumpBuffer:= , 
    bBusy=> , 
    bError=> , 
    ErrorID=> , 
    errorPos=> , 
    ErrorProgramName=> , 
    sentences=> , 
    adwFileSize=> , 
    adwPos=> );

//cnc代码格式转换器功能块 实例
NcIpe(
    sentences:= ReadNC.sentences, 
    bExecute:= GVL.B_ReadFile, 
    bAbort:= , 
    bAppend:= , 
    piStartPosition:= , 
    vStartToolLength:= , 
    nSizeOutQueue:= SIZEOF(IpeBuf), 
    pbyBufferOutQueue:= ADR(IpeBuf), 
    bEnableSyntaxChecks:= , 
    eOriConv:= , 
    dCircleTolerance:= , 
    pInterpreterStack:= , 
    nInterpreterStackSizeBytes:= , 
    bDone=> , 
    bBusy=> , 
    bError=> , 
    wErrorID=> , 
    errorPos=> , 
    poqDataOut=> , 
    iStatus=> , 
    iLineNumberDecoded=> , 
    GCodeText=> , 
    CallstackInfo=> , 
    aActivePrograms=> );


//路径预处理，圆弧平滑叠加， G52激活，G50取消    
RoundPath(
    bExecute:= NcIpe.bDone, 
    bAbort:= , 
    bAppend:= , 
    poqDataIn:= NcIpe.poqDataOut, 
    dRadius:= 10,                                     //圆弧角度
    dAngleTol:= 10,                                    //允许不叠加圆弧的角度。 大于dAngleTol才叠加圆弧 
    nSizeOutQueue:=SIZEOF(RoundPathBuf),  
    pbyBufferOutQueue:=ADR(RoundPathBuf), 
    bDone=> , 
    bBusy=> , 
    bError=> , 
    wErrorID=> , 
    poqDataOut=> );
    
//路径预处理，B样条平滑叠加，G51激活，G50取消
SmoothPath(
    bExecute:= RoundPath.bDone, 
    bAbort:= , 
    bAppend:= , 
    poqDataIn:= RoundPath.poqDataOut, 
    dEdgeDistance:= , 
    dAngleTol:= , 
    nSizeOutQueue:= SIZEOF(SmoothPathBuf),  
    pbyBufferOutQueue:= ADR(SmoothPathBuf), 
    eMode:= , 
    bSymmetricalDistances:= , 
    bImprovedSymmetricCuts:= , 
    eAddAxMode:= , 
    dMinimumCurvatureRadius:= , 
    bCheckCurvature:= , 
    dRelativeCurvatureTol:= , 
    bCheckAddAxVelJump:= , 
    dMaxAddAxVelDifference:= , 
    bDone=> , 
    bBusy=> , 
    bError=> , 
    wErrorID=> , 
    poqDataOut=> , 
    udiStopsDueToCurvatureRadius=> );
    
    
//路径预处理，刀具补偿路径.    G41/G42 激活，G40取消    
ToolCorr(
    bExecute:=  SmoothPath.bDone, 
    bAbort:= , 
    bAppend:= , 
    poqDataIn:= SmoothPath.poqDataOut,  
    dToolRadius:= , 
    nSizeOutQueue:= SIZEOF(ToolCorrBuf),   
    pbyBufferOutQueue:= ADR(ToolCorrBuf), 
    eMode:= , 
    bDone=> , 
    bBusy=> , 
    bError=> , 
    wErrorID=> , 
    poqDataOut=> , 
    iStatus=> );    
    
    
//路径预处理，路径交叉避免.    G61激活，G60取消    
AvoidLoop(
    bExecute:=ToolCorr.bDone,
    bAbort:= , 
    bAppend:= , 
    poqDataIn:= ToolCorr.poqDataOut,
    nSizeOutQueue:= SIZEOF(AvoidLoopBuf),  
    pbyBufferOutQueue:= ADR(AvoidLoopBuf), 
    bDone=> , 
    bBusy=> , 
    bError=> , 
    wErrorID=> , 
    poqDataOut=> , 
    iStatus=> );

//SMC_CheckVelocities的实例
GVL.CheckVel(
    bExecute:= AvoidLoop.bDone, 
    bAbort:= , 
    poqDataIn:= AvoidLoop.poqDataOut, 
    dAngleTol:= , 
    bCheckAddAxVelJump:= , 
    dMaxAddAxVelDifference:= , 
    bBusy=>, 
    bError=> , 
    wErrorID=> , 
    poqDataOut=>);

    
//CheckVel 完成后，复位ReadFile标志
IF GVL.CheckVel.bBusy THEN
    GVL.B_ReadFile:=FALSE;
END_IF

//GVL.CheckVel.bBusy下降沿
CheckVel_F_TRIG(CLK:= GVL.CheckVel.bBusy, Q=> );

GVL.CheckVel.bBusy下降沿,结束ReadFileTask
IF CheckVel_F_TRIG.Q THEN
    GVL.B_ReadFileTask:=FALSE;
END_IF

3.2.2Motion_PRG

//作者:AlongWu

PROGRAM Motion_PRG
VAR

                            
Cnc_Reset                    :BOOL;                    //插补器复位
IsStop                        :BOOL;                    //XYZ三轴停车标志

END_VAR


//Interpolator 插补器实例
GVL.Interpolator(
    bExecute:= GVL.B_Cnc_Ipo, 
    poqDataIn:= GVL.CheckVel.poqDataOut, 
    bSlow_Stop:= , 
    bEmergency_Stop:= (GVL.B_Cnc_Stop OR GVL.B_Cnc_Reset), 
    bWaitAtNextStop:= , 
    dOverride:= 1, 
    iVelMode:= , 
    dwIpoTime:=3000 , 
    dLastWayPos:= , 
    bAbort:= Cnc_Reset, 
    bSingleStep:= , 
    bAcknM:=GVL.B_Cnc_AsknM , 
    bQuick_Stop:= , 
    dQuickDeceleration:= , 
    dJerkMax:= , 
    dQuickStopJerk:= , 
    bSuppressSystemMFunctions:= , 
    bDone=> , 
    bBusy=> , 
    bError=> , 
    wErrorID=> , 
    piSetPosition=> , 
    iStatus=> , 
    bWorking=> , 
    iActObjectSourceNo=> GVL.D_LineNo, 
    dActObjectLength=> , 
    dActObjectLengthRemaining=> , 
    dVel=> , 
    vecActTangent=> , 
    iLastSwitch=> , 
    dwSwitches=> , 
    dWayPos=> , 
    wM=> GVL.W_M_Code, 
    adToolLength=> , 
    Act_Object=> );
    

//TRAFOF_Gantry3D 实例
GVL.TRAFOF_Gantry3D(
    dOffsetX:= , 
    dOffsetY:= , 
    dOffsetZ:= , 
    minX:= 0, 
    maxX:= 200, 
    minY:= 0, 
    maxY:= 200, 
    minZ:= 0, 
    maxZ:= 200, 
    DriveX:= X_Virtual, 
    DriveY:= Y_Virtual, 
    DriveZ:= Z_Virtual, 
    dx=> , 
    dy=> , 
    dz=> , 
    dnx=> , 
    dny=> , 
    dnz=> , 
    dm=> );
    
//TRAFO_Gantry3 实例
GVL.TRAFO_Gantry3(
    pi:= GVL.Interpolator.piSetPosition, 
    dOffsetX:= , 
    dOffsetY:= , 
    dOffsetZ:= , 
    dx=> , 
    dy=> , 
    dz=> );

//插补器动作完成后，复位插补器启动标志
IF GVL.Interpolator.bDone THEN
    GVL.B_Cnc_Ipo :=FALSE;
END_IF
    
//当X,Y,Z速度为0时，赋值IsStop标志
IF GVL.X_Velo = 0  AND GVL.Y_Velo = 0 AND GVL.Z_Velo =0 THEN
    IsStop := TRUE;
ELSE
    IsStop := FALSE;
END_IF


IF GVL.B_Cnc_Reset THEN
    GVL.B_Cnc_Ipo :=FALSE;
    
    // 特别提醒，必须 3轴已经停下来，才能 复位 插补器。不然 虚拟轴会乱跑。
    IF GVL.Interpolator.iStatus = SMC_INT_STATUS.IPO_WAIT AND IsStop THEN
    GVL.B_Cnc_Stop:=FALSE;
    Cnc_Reset := TRUE;
    ELSE
        
        GVL.B_Cnc_Stop:=TRUE;
    END_IF
END_IF


//插补器状态为  Init 时，复位全部标志
IF GVL.Interpolator.iStatus = SMC_INT_STATUS.IPO_INIT THEN
    Cnc_Reset := FALSE;
    GVL.B_Cnc_Reset:=FALSE;
    GVL.B_Cnc_Stop:=FALSE;
    
END_IF

3.2.3PLC_PRG

//作者:AlongWu

PROGRAM PLC_PRG
VAR
    
ReadFileTask_F_TRIG                :F_TRIG;                //读取cnc文件标志下降沿
Cnc_Start_R_TRIG                :R_TRIG;                   //cnc执行标志上升沿


END_VAR

//cnc执行标志上升沿
Cnc_Start_R_TRIG(CLK:= GVL.B_Cnc_Start, Q=> );

IF Cnc_Start_R_TRIG.Q THEN
    //读取cnc文件标志和读取文件Task的触发标志，置TRUE。
    GVL.B_ReadFile:=TRUE;
    GVL.B_ReadFileTask:=TRUE;
END_IF

//读取txt文件标志下降沿
ReadFileTask_F_TRIG(CLK:= GVL.B_ReadFileTask, Q=> );

IF ReadFileTask_F_TRIG.Q THEN
    //读取cnc并解析后，启动插补器
    GVL.B_Cnc_Ipo:= TRUE;
    GVL.B_Cnc_Start :=FALSE;
END_IF

3.2.4Servo_PRG

//作者:AlongWu

PROGRAM Servo_PRG
VAR
    
X_Virtual_Power                :MC_Power;                        //X轴使能功能块 实例
Y_Virtual_Power                :MC_Power;                        //Y轴使能功能块 实例
Z_Virtual_Power                :MC_Power;                        //Z轴使能功能块 实例

AxisXByPos                    :SMC_ControlAxisByPos;            //X轴 位置功能块 实例
AxisYByPos                    :SMC_ControlAxisByPos;            //Y轴 位置功能块 实例
AxisZByPos                    :SMC_ControlAxisByPos;            //Z轴 位置功能块 实例

X_Virtual_Velo                :MC_ReadActualVelocity;            //X轴读取速度功能块 实例
Y_Virtual_Velo                :MC_ReadActualVelocity;            //Y轴读取速度功能块 实例
Z_Virtual_Velo                :MC_ReadActualVelocity;            //Z轴读取速度功能块 实例

X_Virtual_Pos                :MC_ReadActualPosition;            //X轴读取位置功能块 实例
Y_Virtual_Pos                :MC_ReadActualPosition;            //Y轴读取位置功能块 实例
Z_Virtual_Pos                :MC_ReadActualPosition;            //Z轴读取位置功能块 实例

END_VAR


//X轴使能功能块 实例
X_Virtual_Power(
    Axis:= X_Virtual, 
    Enable:= TRUE, 
    bRegulatorOn:= TRUE, 
    bDriveStart:= TRUE, 
    Status=> , 
    bRegulatorRealState=> , 
    bDriveStartRealState=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );
    
//Y轴使能功能块 实例
Y_Virtual_Power(
    Axis:=Y_Virtual , 
    Enable:= TRUE, 
    bRegulatorOn:= TRUE, 
    bDriveStart:= TRUE, 
    Status=> , 
    bRegulatorRealState=> , 
    bDriveStartRealState=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );
//Z轴使能功能块 实例    
Z_Virtual_Power(
    Axis:=Z_Virtual , 
    Enable:= TRUE, 
    bRegulatorOn:= TRUE, 
    bDriveStart:= TRUE, 
    Status=> , 
    bRegulatorRealState=> , 
    bDriveStartRealState=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );        

//X轴读取速度功能块 实例
X_Virtual_Velo(
    Axis:= X_Virtual, 
    Enable:= TRUE, 
    Valid=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> , 
    Velocity=> GVL.X_Velo);
    
//Y轴读取速度功能块 实例
Y_Virtual_Velo(
    Axis:= Y_Virtual, 
    Enable:= TRUE, 
    Valid=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> , 
    Velocity=> GVL.Y_Velo);
    
//Z轴读取速度功能块 实例    
Z_Virtual_Velo(
    Axis:= Z_Virtual, 
    Enable:= TRUE, 
    Valid=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> , 
    Velocity=> GVL.Z_Velo);

//X轴读取位置功能块 实例    
X_Virtual_Pos(
    Axis:= X_Virtual, 
    Enable:= TRUE, 
    Valid=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> , 
    Position=> GVL.X_Pos);    
    
//Y轴读取位置功能块 实例    
Y_Virtual_Pos(
    Axis:= Y_Virtual, 
    Enable:= TRUE, 
    Valid=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> , 
    Position=> GVL.Y_Pos);    
    
//Z轴读取位置功能块 实例
Z_Virtual_Pos(
    Axis:= Z_Virtual, 
    Enable:= TRUE, 
    Valid=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> , 
    Position=> GVL.Z_Pos);        
    
    
    
    
    
    
//X轴 位置功能块 实例    
AxisXByPos(
    Axis:= X_Virtual, 
    iStatus:= GVL.Interpolator.iStatus, 
    bEnable:= GVL.Interpolator.bWorking, 
    bAvoidGaps:= TRUE, 
    fSetPosition:= GVL.TRAFO_Gantry3.dx, 
    fGapVelocity:= 10, 
    fGapAcceleration:= 50, 
    fGapDeceleration:= 50, 
    fGapJerk:= 50, 
    bBusy=> , 
    bCommandAborted=> , 
    bError=> , 
    iErrorID=> , 
    bStopIpo=> );

//Y轴 位置功能块 实例    
AxisYByPos(
    Axis:= Y_Virtual, 
    iStatus:= GVL.Interpolator.iStatus, 
    bEnable:= GVL.Interpolator.bWorking,
    bAvoidGaps:= TRUE, 
    fSetPosition:= GVL.TRAFO_Gantry3.dy, 
    fGapVelocity:= 10, 
    fGapAcceleration:= 50, 
    fGapDeceleration:= 50, 
    fGapJerk:= 50, 
    bBusy=> , 
    bCommandAborted=> , 
    bError=> , 
    iErrorID=> , 
    bStopIpo=> );

//Z轴 位置功能块 实例
AxisZByPos(
    Axis:= Z_Virtual, 
    iStatus:= GVL.Interpolator.iStatus, 
    bEnable:= GVL.Interpolator.bWorking,
    bAvoidGaps:= TRUE, 
    fSetPosition:= GVL.TRAFO_Gantry3.dz, 
    fGapVelocity:= 10, 
    fGapAcceleration:= 50, 
    fGapDeceleration:= 50, 
    fGapJerk:= 50, 
    bBusy=> , 
    bCommandAborted=> , 
    bError=> , 
    iErrorID=> , 
    bStopIpo=> );

3.2.5MsgReplyFrame_FB

//作者:AlongWu

FUNCTION_BLOCK MsgReplyFrame_FB
VAR_INPUT
ReplyBuffer                    :POINTER TO BYTE;                       //输出buffer
DataBuffer                    :POINTER TO BYTE;                        //数据buffer
DataByteLen                    :UINT;                                  //数据buffer的byte长度
FrameType                    :BYTE;                                    //帧类型 
    
END_VAR
VAR_OUTPUT
BufferBytes                    :UINT;                                    //字节数
END_VAR
VAR
TmpCrc                        :WORD;                                    //crc校验码
UnpackWord1                 :MEM.UnpackWord;                        //unpackword功能块实例

END_VAR


    BufferBytes := 5+DataByteLen;
    
    UnpackWord1(    wValue:= BufferBytes, byLowByte=> ReplyBuffer[0] , byHighByte=>ReplyBuffer[1]);                            //帧字节总数    
    ReplyBuffer[2] :=FrameType;                                                                                                //帧类型
    

    MEM.MemMove(pSource:= DataBuffer, pDestination:= ReplyBuffer+3, uiNumberOfBytes:=DataByteLen );                            //字符复制
    
    TmpCrc := MEM.CRC16_Modbus(pMemoryBlock:=ReplyBuffer , uiLength:=BufferBytes-2 );                                        //crc校验        
    UnpackWord1(    wValue:= TmpCrc, byLowByte=> ReplyBuffer[BufferBytes-2] , byHighByte=>ReplyBuffer[BufferBytes-1]);        //crc赋值

3.2.6UdpDecode_PRG

//作者:AlongWu

PROGRAM UdpDecode_PRG
VAR CONSTANT
emptyReceiveBuffer                :ARRAY [0..1023] OF BYTE;                //复位ReceiveBuffer
END_VAR


VAR
MsgReplyFr                :MsgReplyFrame_FB;                               //回复帧的功能块实例
BInfo                    :BasicInfo;                                       //当前状态的结构体实例
TxtWrite                :TxtWrite_FB;                                      //txt写入功能块实例
TxtTruncate                :TxtTruncate_FB;                                //txt清空内容功能块实例
iecResult                  : SysTypes.RTS_IEC_RESULT;                      //file操作的result
    
i                        :INT;                                             //循环i
TmpCrc                    :WORD;                                           //计算crc
ReceiBytes                :UINT;                                           //接收的字节数
CheckCrc                :WORD;                                             //接收数据中crc
CheckFrameLen            :UINT;                                            //帧字节总数
DataLen                    :UINT;                                          //数据字节数
AskCode                    :BYTE;                                          //命令码
ReceiveBuffer            :ARRAY[0..1023] OF BYTE;                          //接收数据的缓存
B_Reply                    :BOOL;                                          //回复动作标志
tmpFileId                :BYTE;                                            //接收文件时发送批次id

END_VAR

(*
    UDP通信 

    PC发送的帧结构
    
    2B: 发送字节数， 含CRC, UINT
    1B: 查询码,UINT
    NB: 数据内容
    2B: CRC校验码

*)



ReceiBytes := UDINT_TO_UINT(GVL.UdpReceiveBytes);

MEM.MemMove(pSource:= ADR(GVL.UdpReceiveBuffer), pDestination:= ADR(ReceiveBuffer), uiNumberOfBytes:=ReceiBytes );                    //字符复制

GVL.UdpReplyIpAddr := GVL.UdpReceiIpAddr; 
GVL.UdpReplyPort := GVL.UdpReceiPort;

B_Reply :=FALSE;

IF ReceiBytes > 1023 OR ReceiBytes = 0 THEN
    //帧字符数超限
    //nothing
ELSE
    //帧字符数正常
    DataLen := WORD_TO_UINT(MEM.PackBytesToWord(byHighByte:=GVL.UdpReceiveBuffer[1] , byLowByte:= GVL.UdpReceiveBuffer[0]));    //前2B，帧大小的值
    
    CheckFrameLen := UDINT_TO_UINT(GVL.UdpReceiveBytes);                                                                        //实际接收的字节总数
        
    TmpCrc := MEM.CRC16_Modbus(pMemoryBlock:=ADR(GVL.UdpReceiveBuffer) , uiLength:=CheckFrameLen-2 );                            //接收数据生成crc
    
    CheckCrc := MEM.PackBytesToWord(byHighByte:=GVL.UdpReceiveBuffer[CheckFrameLen-1] , byLowByte:= GVL.UdpReceiveBuffer[CheckFrameLen-2]);    //提取接收数据中的crc
    
    IF    CheckFrameLen = DataLen THEN
        //帧大小与实际接收字节数相同
        IF TmpCrc = CheckCrc THEN
            //crc校验通过
        AskCode := GVL.UdpReceiveBuffer[2];
        CASE AskCode OF 
                1:
                    //读取当前状态
                        BInfo.X_Pos := LREAL_TO_REAL(GVL.X_Pos);
                        BInfo.Y_Pos := LREAL_TO_REAL(GVL.Y_Pos);
                        BInfo.Z_Pos := LREAL_TO_REAL(GVL.Z_Pos);
                        BInfo.LineNo := DINT_TO_INT(GVL.D_LineNo);
                        BInfo.wM :=GVL.Interpolator.wM;            
                        BInfo.State.0 := GVL.Interpolator.bBusy;
                        BInfo.State.1 :=GVL.Interpolator.bDone;
                        BInfo.State.2 :=GVL.Interpolator.bAcknM;
                        
                    
                        B_Reply :=TRUE;
                        MsgReplyFr(    ReplyBuffer:= ADR(GVL.UdpReplyBuffer),     DataBuffer:= ADR(BInfo) , DataByteLen:= 18,     FrameType:= 1,     BufferBytes=>GVL.UdpReplyBytes);
                2:
                    //启动cnc    
                    IF GVL.B_Cnc_Stop THEN
                        GVL.B_Cnc_Stop :=FALSE;
                    ELSE        
                        IF GVL.B_Cnc_Ipo = FALSE THEN
                        B_Reply :=TRUE;
                        GVL.B_Cnc_Start:=TRUE;
                        MEM.MemMove(pSource:= ADR(GVL.UdpReceiveBuffer), pDestination:= ADR(GVL.UdpReplyBuffer), uiNumberOfBytes:=CheckFrameLen );        
                        END_IF        
                    END_IF    
                3:
                    //停止cnc
                        B_Reply :=TRUE;
                        GVL.B_Cnc_Stop:=TRUE;
                        MEM.MemMove(pSource:= ADR(GVL.UdpReceiveBuffer), pDestination:= ADR(GVL.UdpReplyBuffer), uiNumberOfBytes:=CheckFrameLen );    
                4:
                    //重置cnc
                        B_Reply :=TRUE;
                        GVL.B_Cnc_Reset:=TRUE;
                        MEM.MemMove(pSource:= ADR(GVL.UdpReceiveBuffer), pDestination:= ADR(GVL.UdpReplyBuffer), uiNumberOfBytes:=CheckFrameLen );    
            
                10:
                    //启动接收cnc代码
                        B_Reply :=TRUE;                        
                        TxtTruncate();            //打开固定文件，并且清空文件内容。                    
                        MEM.MemMove(pSource:= ADR(GVL.UdpReceiveBuffer), pDestination:= ADR(GVL.UdpReplyBuffer), uiNumberOfBytes:=CheckFrameLen );    
                        tmpFileId:=0;                    
                11://开始接收cnc代码
                        IF tmpFileId = GVL.UdpReceiveBuffer[3] THEN
                            //检查接收的帧id是否正确
                            B_Reply :=TRUE;                        
                            TxtWrite(Buffer:=ADR(ReceiveBuffer)+4, BufferLen:= CheckFrameLen-6);            //打开固定文件，续写文件。                                        
                            MsgReplyFr(    ReplyBuffer:= ADR(GVL.UdpReplyBuffer),     DataBuffer:= ADR(tmpFileId) , DataByteLen:= 1,     FrameType:= 11,     BufferBytes=>GVL.UdpReplyBytes);
                            tmpFileId := tmpFileId +1;
                        ELSE
                            //帧id不连续，发送FF码。
                            B_Reply :=TRUE;            
                            tmpFileId := 0;
                            MsgReplyFr(    ReplyBuffer:= ADR(GVL.UdpReplyBuffer),     DataBuffer:= ADR(tmpFileId) , DataByteLen:= 1,     FrameType:= 16#FF,     BufferBytes=>GVL.UdpReplyBytes);                
                        END_IF
                20:
                    //发送cnc代码给PC
                    //读取文件字节数
                    GVL.UD_FileSize :=SysFileGetSize(szFileName:= GVL.STR_CncFilePath, pResult:= ADR(iecResult));
                    GVL.UD_FileOffSet := 0;
                    GVL.B_UDP_FileSend := TRUE;                    
                ELSE
                    //查询码错误
                    //nothing
            END_CASE            
        ELSE
        //crc校验错误
        //nothing
        END_IF    
    END_IF

END_IF
 

GVL.UdpReceiveBuffer := emptyReceiveBuffer;                                                                //复位recei的buffer

IF B_Reply THEN
    GVL.UdpReplySendSig := TRUE;                                                                        //触发Udp_PRG的send实例
END_IF

3.2.7Udp_PRG

//作者:AlongWu
PROGRAM Udp_PRG
VAR
    
ReplyIpAddr                    :NBS.IP_ADDR;                                 //udp来源方ip地址
ReplyPort                    :UINT;                                          //udp来源方port   
UdpSend                        :NBS.UDP_Send;                                //udp发送功能块实例
UdpServerIpAddrMulti        :NBS.IP_ADDR := (sAddr := '255.255.255.255');    //udpserver的本身ip地址


TxtRead                        :TxtRead_FB;                                //txt读取功能块实例
FileSizeTmp                    :UDINT;                                     //文件读取剩余字节数
FileReadBuff                :ARRAY[0..1023] OF BYTE;                       //txt读取的字节缓存
MsgReplyFr                    :MsgReplyFrame_FB;                           //回复帧的功能块实例

END_VAR
(*updserver和read*)

//udp功能块实例
GVL.UdpServer(
    xEnable:= TRUE, 
    xDone=> , 
    xBusy=> , 
    xError=> , 
    ipAddr:= GVL.UdpServerIpAddr, 
    uiPort:= GVL.UdpServerPort, 
    ipMultiCast:= UdpServerIpAddrMulti, 
    eError=> , 
    xActive=> , 
    hPeer=> );
    
//udp接收功能块实例
GVL.UdpReceive(
    xEnable:=GVL.UdpServer.xBusy , 
    xDone=> , 
    xBusy=> , 
    xError=> , 
    hPeer:= GVL.UdpServer.hPeer, 
    szSize:= SIZEOF(GVL.UdpReceiveBuffer), 
    pData:= ADR(GVL.UdpReceiveBuffer), 
    eError=> , 
    xReady=> GVL.UdpReceivedSig, 
    ipFrom=> GVL.UdpReceiIpAddr, 
    uiPortFrom=>GVL.UdpReceiPort, 
    szCount=>GVL.UdpReceiveBytes);
    

(*通过GVL.fbUdpReceive的ready信号，触发UdpInterrupt_everntTask,中断程序解析指令，*)
    
//udp发送功能块实例    
UdpSend(
    xExecute:=GVL.UdpReplySendSig , 
    udiTimeOut:= 100, 
    xDone=> , 
    xBusy=> , 
    xError=> , 
    hPeer:= GVL.UdpServer.hPeer, 
    ipAddr:= GVL.UdpReplyIpAddr , 
    uiPort:= GVL.UdpReplyPort , 
    szSize:= GVL.UdpReplyBytes, 
    pData:= ADR(GVL.UdpReplyBuffer), 
    eError=> );
    
    IF UdpSend.xDone THEN
        GVL.UdpReplySendSig :=FALSE;
    END_IF
    
    

(*发送cnc代码文件*)
    IF GVL.B_UDP_FileSend THEN
        IF GVL.UdpReplySendSig = FALSE THEN
        //更新文件读取剩余字节数
        FileSizeTmp := GVL.UD_FileSize  - GVL.UD_FileOffSet;
            IF FileSizeTmp >= 1000 THEN        
                    //剩余字节大于1000，按1000字节内容大小发送            
                    TxtRead(ulOffset:= GVL.UD_FileOffSet,BufferLen:=1000,Buffer=> FileReadBuff);
                    GVL.UD_FileOffSet := GVL.UD_FileOffSet+1000;
                    MsgReplyFr(    ReplyBuffer:= ADR(GVL.UdpReplyBuffer),     DataBuffer:= ADR(FileReadBuff) , DataByteLen:= 1000,     FrameType:= 21,     BufferBytes=>GVL.UdpReplyBytes);                
                    GVL.UdpReplySendSig:= TRUE;
            ELSE
                IF     FileSizeTmp > 0 THEN
                    //剩余字节小于1000，按实际字节内容大小发送    
                    TxtRead(ulOffset:= GVL.UD_FileOffSet,BufferLen:=UDINT_TO_UINT(FileSizeTmp),Buffer=> FileReadBuff);
                    GVL.UD_FileOffSet := GVL.UD_FileOffSet+FileSizeTmp;
                    MsgReplyFr(    ReplyBuffer:= ADR(GVL.UdpReplyBuffer),     DataBuffer:= ADR(FileReadBuff) , DataByteLen:= UDINT_TO_UINT(FileSizeTmp),     FrameType:= 21,     BufferBytes=>GVL.UdpReplyBytes);                
                    GVL.UdpReplySendSig:= TRUE;
                ELSE
                    //剩余字节数为0，结束发送
                    GVL.B_UDP_FileSend :=FALSE;
                END_IF
            END_IF
        END_IF
    END_IF

3.2.8TxtRead_FB

//作者:AlongWu
FUNCTION_BLOCK TxtRead_FB
VAR_INPUT
ulOffset        :UDINT;                        //文件的读取开始位置
BufferLen        :UINT;                        //文件读取的字节数
END_VAR
VAR_OUTPUT
    
Buffer            :ARRAY[0..1023] OF BYTE;    //文件读取结果

END_VAR
VAR
    
hFile          : SysTypes.RTS_IEC_HANDLE := SysTypes.RTS_INVALID_HANDLE;   //file操作的权柄
iecResult      : SysTypes.RTS_IEC_RESULT;                                  //file操作的result
udiRead       : __XWORD;                                                   //file读取最终字节数
END_VAR

hFile := SysFileOpen(szFile:=GVL.STR_CncFilePath, am:= SYSFILE.AM_READ , pResult:=ADR(iecResult));                    //打开文件，设置打开类型

IF hFile <> RTS_INVALID_HANDLE    THEN
    SysFileSetPos(hFile:= hFile, ulOffset:= ulOffset);                                                                //设置读取位置
     udiRead := SysFileRead(hFile:= hFile, pbyBuffer:=ADR(Buffer) , ulSize:= BufferLen, pResult:=ADR(iecResult) );    //读取文件
    iecResult  := SysFileClose(hFile:=hFile);                                                                        //关闭file权柄
END_IF

3.2.9TxtTruncate_FB

//作者:AlongWu
FUNCTION_BLOCK TxtTruncate_FB
VAR_INPUT
END_VAR
VAR_OUTPUT
END_VAR
VAR
    
hFile          : SysTypes.RTS_IEC_HANDLE := SysTypes.RTS_INVALID_HANDLE;     //file操作的权柄
iecResult      : SysTypes.RTS_IEC_RESULT;                                    //file操作的result
udiTrun       : __XWORD;                                                     //反馈字节数


END_VAR
hFile := SysFileOpen(szFile:=GVL.STR_CncFilePath, am:=SYSFILE.AM_WRITE , pResult:=ADR(iecResult));    //打开文件，设置打开类型

IF hFile <> RTS_INVALID_HANDLE    THEN
     udiTrun := SysFileTruncate(hFile:= hFile, ulSizeNew:=0 );                                        //文件大小设置为0，即清空内容
    iecResult  := SysFileClose(hFile:=hFile);                                                        //关闭file权柄
END_IF

3.2.10TxtWrite_FB

//作者:AlongWu
FUNCTION_BLOCK TxtWrite_FB
VAR_INPUT

Buffer            :POINTER TO ARRAY[0..1023] OF BYTE;                                        //写入字节内容
BufferLen        :UDINT;                                                                    //写入字节数量
END_VAR
VAR_OUTPUT
END_VAR
VAR
hFile          : SysTypes.RTS_IEC_HANDLE := SysTypes.RTS_INVALID_HANDLE;    //file操作的权柄
iecResult      : SysTypes.RTS_IEC_RESULT;                                   //file操作的result
udiWrite       : __XWORD;                                                   //反馈字节数


END_VAR
    
hFile := SysFileOpen(szFile:=GVL.STR_CncFilePath, am:=SYSFILE.AM_APPEND, pResult:=ADR(iecResult));                    //打开文件，设置续写模式

IF hFile <> RTS_INVALID_HANDLE    THEN
     udiWrite := SysFileWrite(hFile:=hFile, pbyBuffer:=Buffer, ulSize:=BufferLen, pResult:=ADR(iecResult));            //写文件 
    iecResult  := SysFileClose(hFile:=hFile);                                                                        //关闭file权柄
END_IF

四、总结

本文的程序，主要实现的功能是，通过上位机程序，利用Udp通信手段去控制和使用Codesys基于读和解析CNC的G代码文件的功能并实施G代码动作。

本文的程序构建了一个简易的通信协议。上位机程序通过该通信协议与Codesys的程序进行交互指令。

WPF上位机程序，使用了MVVM模式。本文展示了实现各主要功能的MainCommand的代码。该程序开启时，绑定udp并且启动Task，循环访问Codesys程序当前的状态。使用时，界面的按钮会通过udp发送相应指令来通知Codesys相应动作。

Codesys程序任务为：

4个定时任务

Main_Task(100ms,优先级8)

Motion_Task(10ms,优先级8)

Servo_Task(1ms,优先级1)

Udp_Task(10ms,优先级10)

1个StatusTask

CncDecode_StatusTask(优先级15)

1个EventTask

UdpDecode_EventTask(优先级8)

CncDecode只有从头执行G代码才需要读取和解析文件，而且解析G代码的功能块需要一定运行时间，因此用了StatusTask。

UdpDecode 每次udp接收后才做的动作，而且解析的动作为一次性的，因此用了EventTask。

本程序的在解析G代码中，加入了4个Codesys自带的路径预处理功能块。分别为：圆弧平滑叠加，B样条平滑叠加，刀具补偿路径，路径交叉避免。

AlongWu1122

关注

26
点赞
踩
51

收藏

觉得还不错? 一键收藏
5
评论
WPF上位机+Codesys的CNC

利用C#的WPF做上位机，通过Udp与Codesys通信并利用Codesys的File和Cnc模块，传输CNC的G代码文件和执行G代码。本程序展示了，Codesys的Cnc解析和执行G代码的完整过程、Txt读写文件操作过程以及Udp通信交互过程。
复制链接

扫一扫