CyberX8/CyberX8_Simulator/Devices/WagoSocketSimulator.cs

using Aitex.Common.Util;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.Util;
using MECF.Framework.Common.Device.Festo;
using MECF.Framework.Common.Device.Wago;
using MECF.Framework.Common.Net;
using MECF.Framework.Common.Simulator;
using MECF.Framework.Simulator.Core.Driver;
using System;
using System.Collections.Generic;
using System.IO;

namespace CyberX8_Simulator.Devices
{
    public class WagoSocketSimulator : SocketDeviceSimulator
    {
        private const short WRITE_DO_STARTADDRESS = 0x0200;
        private const short WRITE_AO_STARTADDRESS = 0x0200;

        //键是名字，值是对应数据所在的位置  注意：要和WagoControlCfg里面的地址顺序对上
        public Dictionary<string, int> DONameIndexDic;

        public Dictionary<string, int> DINameIndexDic;
        
        public Dictionary<string, int> AINameIndexDic;
        
        public Dictionary<string, int> AONameIndexDic;

        private IByteTransform byteTransform = new BigEndianByteTransformBase();

        //存储模拟器数据的数组
        public byte[] DOBytes = new byte[100];

        public short[] AOShorts = new short[100];

        public byte[] DIBytes = new byte[100];

        public short[] AIShorts = new short[100];

        /// <summary>
        /// 写DO锁
        /// </summary>
        private object _writeDOLocker = new object();
        /// <summary>
        /// 写AO锁
        /// </summary>
        private object _writeAOLocker = new object();
        /// <summary>
        /// Festo Data Buffer
        /// </summary>
        private Dictionary<string, bool> _festoDataBuffer = new Dictionary<string, bool>();
        /// <summary>
        /// 定时器
        /// </summary>
        private PeriodicJob _periodicJob;

        //delegate
        #region Delegate
        public delegate void VariableValueChanged(object obj);
        #endregion

        #region 事件
        /// <summary>
        /// 变量变更事件
        /// </summary>
        public event VariableValueChanged OnDIVariableValueChanged;
        public event VariableValueChanged OnAIVariableValueChanged;
        public event VariableValueChanged OnDOVariableValueChanged;
        public event VariableValueChanged OnAOVariableValueChanged;
        #endregion

        public WagoSocketSimulator(int port):base(port) 
        {
            SimulatorCommManager.Instance.OnUpdateVariableValueChanged += UpdataDataCausedByOtherModule;
            MotorSimulator.Instance.OnUpdateWagoDatasChanged += UpdataDataCausedByOtherModule;
            InitializeData(port);    
        }  
        
        private void UpdataDataCausedByOtherModule(string name, bool value, bool invert)
        {
            value = invert ? !value : value;
            //AI Data
            if (AINameIndexDic.ContainsKey(name))
            {
                switch (name)
                {
                    case "r_LoaderA_LS_Vacuum_anlg":
                    case "r_LoaderB_LS_Vacuum_anlg":
                        AIShorts[AINameIndexDic[name]] = value ? (short)0x32C8 : (short)0x2AF8;
                        break;
                    case "r_DPUF_A_CHUCK_A_VAC":
                    case "r_DPUF_A_CHUCK_B_VAC":
                        AIShorts[AINameIndexDic[name]] = value ? (short)0x0C80 : (short)0x32C8;
                        break;
                    case "r_LOADERA_BERNOULLI_PRESSURE":
                    case "r_LOADERB_BERNOULLI_PRESSURE":
                    case "r_LOADERA_CHUCK_BLADDER":
                    case "r_LOADERB_CHUCK_BLADDER":
                    case "r_LOADERA_WS_BLADDER_PRESSURE":
                    case "r_LOADERB_WS_BLADDER_PRESSURE":
                        AIShorts[AINameIndexDic[name]] = value ? (short)0x2AF8 : (short)0x00;
                        break;
                    default:
                        break;
                }          
            }
            //DI Data
            UpdataDIBytes(name, value ? 1 : 0);
            //Festo Data
            if (name == "FlowTestClamp") _festoDataBuffer[name] = value;
        }
        /// <summary>
        /// 触发Wago对应数据更新
        /// </summary>
        /// <param name="position"></param>
        /// <param name="value"></param>
        private void UpdateDataCausedByWago(int position, bool value)
        {
            if (DONameIndexDic.ContainsKey("c_PUF_CHUCK") && position == DONameIndexDic["c_PUF_CHUCK"])
            {
                UpdataDIBytes("r_PUF_A_CHUCK_OUT", value ? 1 : 0);
                UpdataDIBytes("r_PUF_B_CHUCK_OUT", value ? 1 : 0);
                UpdataDIBytes("r_PUF_A_CHUCK_IN", !value ? 1 : 0);
                UpdataDIBytes("r_PUF_B_CHUCK_IN", !value ? 1 : 0);                             
            }
            if (DONameIndexDic.ContainsKey("c_HVD_1_HIGH") && position == DONameIndexDic["c_HVD_1_HIGH"])
            {
                UpdataAIShorts("r_HVD_1_ANALOG", value ? 4500 : 0);
            }

            if (DONameIndexDic.ContainsKey("c_HVD_2_HIGH") && position == DONameIndexDic["c_HVD_2_HIGH"])
            {
                UpdataAIShorts("r_HVD_2_ANALOG", value ? 4500 : 0);
            }
            
        }
        /// <summary>
        /// 初始化字典
        /// </summary>
        private void InitializeData(int port)  //端口用于初始化不同Wago设备的字典
        {
            //加载对应配置文件 WagoControllerCfg-Simulator.xml
            try
            {
                string oldXmlPath = PathManager.GetCfgDir();
                string newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Devices\\WagoControllerCfg-Simulator.xml";
                WagoControllerCfg cfg = CustomXmlSerializer.Deserialize<WagoControllerCfg>(new FileInfo(newXmlPath));
                if (cfg != null)
                {
                    foreach (WagoDeviceConfig config in cfg.WagoDeviceConfigs)
                    {
                        if (port == config.Port)
                        {
                            if(config.Module == "Loader" && _periodicJob == null)
                            {
                                _periodicJob = new PeriodicJob(100, OnTimer, $"Wago {config.Module} timer",true);
                            }
                            //加载DO
                            int i = 0;
                            DONameIndexDic = new Dictionary<string, int>();
                            if(config.WagoDigOut != null && config.WagoDigOut.WagoDOGroups != null)
                            {
                                foreach (var group in config.WagoDigOut.WagoDOGroups)
                                {
                                    foreach (var item in group.WagoDOs)
                                    {
                                        DONameIndexDic[item.Name] = i;
                                        i++;
                                    }
                                }
                            }
                            //加载DI
                            i = 0;
                            DINameIndexDic = new Dictionary<string, int>();
                            if(config.WagoDigIn != null && config.WagoDigIn.WagoDIGroups != null)
                            {
                                foreach (var group in config.WagoDigIn.WagoDIGroups)
                                {
                                    foreach (var item in group.WagoDIs)
                                    {
                                        DINameIndexDic[item.Name] = i;
                                        i++;
                                    }
                                }
                            }
                            //加载AO
                            i = 0;
                            AONameIndexDic = new Dictionary<string, int>();
                            if (config.WagoAnoOut != null && config.WagoAnoOut.WagoAOGroups != null)
                            {
                                foreach (var group in config.WagoAnoOut.WagoAOGroups)
                                {
                                    foreach (var item in group.WagoAOs)
                                    {
                                        AONameIndexDic[item.Name] = i;
                                        i++;
                                    }
                                }
                            }
                            //加载AI
                            i = 0;
                            AINameIndexDic = new Dictionary<string, int>();
                            if (config.WagoAnoIn != null && config.WagoAnoIn.WagoAIGroups != null)
                            {
                                foreach (var group in config.WagoAnoIn.WagoAIGroups)
                                {
                                    foreach (var item in group.WagoAIs)
                                    {
                                        AINameIndexDic[item.Name] = i;
                                        i++;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            catch
            {
                LOG.WriteLog(eEvent.ERR_WAGO, "Wago", "Load wago WagoControllerCfg-Simulator.xml failed");
            }
            
            //设置IO变量默认值
            if (AINameIndexDic.ContainsKey("r_LoaderA_LS_Vacuum_anlg")) AIShorts[AINameIndexDic["r_LoaderA_LS_Vacuum_anlg"]] = 0x32C8;
            if (AINameIndexDic.ContainsKey("r_LoaderB_LS_Vacuum_anlg")) AIShorts[AINameIndexDic["r_LoaderB_LS_Vacuum_anlg"]] = 0x32C8;
            if (AINameIndexDic.ContainsKey("r_DPUF_A_CHUCK_A_VAC")) AIShorts[AINameIndexDic["r_DPUF_A_CHUCK_A_VAC"]] = 0x32C8;
            if (AINameIndexDic.ContainsKey("r_DPUF_A_CHUCK_B_VAC")) AIShorts[AINameIndexDic["r_DPUF_A_CHUCK_B_VAC"]] = 0x32C8;
            if (AINameIndexDic.ContainsKey("r_SYSTEM_EXHAUST")) AIShorts[AINameIndexDic["r_SYSTEM_EXHAUST"]] = 0x3A98;
            if (DONameIndexDic.ContainsKey("c_HVD_1_ENABLE")) DOBytes[DONameIndexDic["c_HVD_1_ENABLE"]] = 1;
            if (DONameIndexDic.ContainsKey("c_HVD_2_ENABLE")) DOBytes[DONameIndexDic["c_HVD_2_ENABLE"]] = 1;
        }
        #region 公共方法
        public void UpdataDOBytes(string name,int value)
        {
            if (OnDOVariableValueChanged != null)
            {
                OnDOVariableValueChanged(name);
            }
            if (DONameIndexDic.ContainsKey(name))
            {
                if (DONameIndexDic[name] < DOBytes.Length)
                {
                    DOBytes[DONameIndexDic[name]] = value == 0 ? (byte)0 : (byte)1;
                }
            }
        }
        public void UpdataDIBytes(string name, int value)
        {
            if (OnDIVariableValueChanged != null)
            {
                OnDIVariableValueChanged(name);
            }

            if (DINameIndexDic.ContainsKey(name))
            {
                if (DINameIndexDic[name] < DIBytes.Length)
                {
                    DIBytes[DINameIndexDic[name]] = value == 0 ? (byte)0 : (byte)1;
                }
            }
        }
        public void UpdataAOShorts(string name, int value)
        {
            if (OnAOVariableValueChanged != null)
            {
                OnAOVariableValueChanged(name);
            }
            if (AONameIndexDic.ContainsKey(name))
            {
                string hexValue = value.ToString("X2");
                try
                {
                    short result = Convert.ToInt16(hexValue, 16);
                    if (AONameIndexDic[name] < AOShorts.Length)
                    {
                        AOShorts[AONameIndexDic[name]] = result;
                    }
                }
                catch (FormatException)
                {

                }
            }
        }
        public void UpdataAIShorts(string name, int value)
        {
            if (OnAIVariableValueChanged != null)
            {
                OnAIVariableValueChanged(name);
            }
            if (AINameIndexDic.ContainsKey(name))
            {
                string hexValue = value.ToString("X2");
                try
                {
                    short result = Convert.ToInt16(hexValue, 16);
                    if(AINameIndexDic[name] < AIShorts.Length)
                    {
                        AIShorts[AINameIndexDic[name]] = result;
                    }
                }
                catch (FormatException)
                {
                   
                }
            }
        }
        #endregion

        #region 功能方法
        /// <summary>
        /// 将长度为8的二进制byte数组转成对应十六进制byte值（大端模式）
        /// </summary>
        /// <param name="byteArray"></param>
        /// <returns></returns>
        public byte ConvertByteArrayToHex(byte[] byteArray)
        {
            byte result = 0;
            // 先将 byte 数组转换为二进制数
            int binaryValue = 0;
            for (int i = 0; i < 8; i++)
            {  
                binaryValue |= (byteArray[i] << (7 - i));
            }

            // 逆转二进制数
            int reversedValue = 0;
            for (int i = 0; i < 8; i++)
            {
                reversedValue |= ((binaryValue >> i) & 1) << (7 - i);
            }
            // 转换为十六进制byte
            if (byte.TryParse(reversedValue.ToString("X2"), System.Globalization.NumberStyles.HexNumber, null, out result))
            {
               return result;
            }
            return 0;
        }
        /// <summary>
        /// 将short数组转成长度两倍的byte数组
        /// </summary>
        /// <param name="shortArray"></param>
        /// <returns></returns>
        private byte[] ConvertShortArrayToByteArray(short[] shortArray)
        {
            byte[] byteArray = new byte[shortArray.Length * 2];
            for (int i = 0; i < shortArray.Length; i++)
            {
                byte[] tempBytes = BitConverter.GetBytes(shortArray[i]);
                Array.Reverse(tempBytes);
                Array.Copy(tempBytes, 0, byteArray, i * 2, 2);
            }
            return byteArray;
        }
        #endregion
        
        protected override void ProcessUnsplitMessage(byte[] data)
        {
            byte command = data[7];
            if (command == 0x01) //读DO
            {
                short flag = byteTransform.TransInt16(data, 0);
                byte channel = data[6];
                short startAddress = byteTransform.TransInt16(data, 8);
                short bitCount = byteTransform.TransInt16(data, 10);
                byte byteCount = (byte)(bitCount / 8 + 1);
                byte[] bytes = new byte[byteCount];
                for(int i = 0; i < byteCount;i++)
                {
                    byte[] tempbytes = new byte[8];
                    Array.Copy(DOBytes,8 * i, tempbytes, 0, 8);
                    bytes[i] = ConvertByteArrayToHex(tempbytes);
                }
                OnWriteMessage(CreateReadDigitalResponse(flag, channel, command, byteCount, bytes));
                return;
            }
            else if(command == 0x03)//读AO
            {
                short flag = byteTransform.TransInt16(data, 0);
                byte channel = data[6];
                short startAddress = byteTransform.TransInt16(data, 8);
                short registerCount = byteTransform.TransInt16(data, 10);
                short[] shorts = new short[registerCount];//获取指定寄存器里的内容
                Array.Copy(AOShorts, 0, shorts, 0, registerCount);
                byte[] bytes = new byte[registerCount * 2];
                bytes = ConvertShortArrayToByteArray(shorts); //转入长度为shorts数组长度两倍的bytes数组中
                OnWriteMessage(CreateReadAnalogyResponse(flag, channel, command, (byte)registerCount, bytes));
                return;
            }
            else if (command == 0x02)//读DI
            {
                short flag = byteTransform.TransInt16(data, 0);
                byte channel = data[6];
                short startAddress = byteTransform.TransInt16(data, 8);
                short bitCount = byteTransform.TransInt16(data, 10);
                byte byteCount = (byte)(bitCount / 8 + 1);
                byte[] bytes = new byte[byteCount];
                for (int i = 0; i < byteCount; i++)
                {
                    byte[] tempbytes = new byte[8];
                    Array.Copy(DIBytes, 8 * i, tempbytes, 0, 8);
                    bytes[i] = ConvertByteArrayToHex(tempbytes);
                }
                OnWriteMessage(CreateReadDigitalResponse(flag, channel, command, byteCount, bytes));
                return;
            }
            else if (command == 0x04)//读AI
            {
                short flag = byteTransform.TransInt16(data, 0);
                byte channel = data[6];
                short startAddress = byteTransform.TransInt16(data, 8);
                short registerCount = byteTransform.TransInt16(data, 10);
                short[] shorts = new short[registerCount];//获取指定寄存器里的内容
                Array.Copy(AIShorts, 0, shorts, 0, registerCount);
                byte[] bytes = new byte[registerCount * 2];
                bytes = ConvertShortArrayToByteArray(shorts); //转入长度为shorts数组两倍的bytes数组中
                OnWriteMessage(CreateReadAnalogyResponse(flag, channel, command, (byte)registerCount, bytes));
                return;
            }
            else if (command == 0x05)//写DO
            {
                short startAddress = byteTransform.TransInt16(data, 8);
                if (startAddress > 0x03FF || startAddress < WRITE_DO_STARTADDRESS)
                {
                    short flag = byteTransform.TransInt16(data, 0);
                    byte channel = data[6];
                    OnWriteMessage(CreateError(flag, channel, command, 0x02)); //地址错误
                    return;
                }
                int position = startAddress - WRITE_DO_STARTADDRESS;
                bool status = data[10] == 0xFF ? true : false;
                lock (_writeDOLocker)
                {
                    DOBytes[position] = status ? (byte)1 : (byte)0;
                }
                OnWriteMessage(data); //原消息返回
                //触发Wago对应数据更新
                UpdateDataCausedByWago(position, status);
                return;
            }
            else if (command == 0x06)//写AO
            {
                short startAddress = byteTransform.TransInt16(data, 8);
                if(startAddress > 0x02FF || startAddress < WRITE_AO_STARTADDRESS)
                {
                    short flag = byteTransform.TransInt16(data, 0);
                    byte channel = data[6];
                    OnWriteMessage(CreateError(flag, channel, command, 0x02)); //地址错误
                    return;
                }
                int position = startAddress - WRITE_AO_STARTADDRESS;
                short value = byteTransform.TransInt16(data, 10);
                lock (_writeAOLocker)
                {
                    AOShorts[position] = value;
                }
                OnWriteMessage(data); //原消息返回
                return;
            }
            else 
            {
                short flag = byteTransform.TransInt16(data, 0);
                byte channel = data[6];
                OnWriteMessage(CreateError(flag, channel, command, 0x01)); //指令错误
                return;
            }
        }
        /// <summary>
        /// 回复读数字量
        /// </summary>
        /// <param name="flag"></param>
        /// <param name="channel"></param>
        /// <param name="command"></param>
        /// <param name="byteCount"></param>
        /// <param name="values"></param>
        /// <returns></returns>
        private byte[] CreateReadDigitalResponse(short flag, byte channel, byte command, byte byteCount, byte[] values)
        {
            byte[] bytes = new byte[7 + 2 + values.Length]; //回复字节长度，前面7个字节固定长度 + functionCode一个字节 + byteCount一个字节+values.length个字节
            Array.Copy(byteTransform.GetBytes(flag), 0, bytes, 0, 2);
            bytes[2] = 0x00;
            bytes[3] = 0x00;
            short dataLength = (short)(3 + values.Length);
            Array.Copy(byteTransform.GetBytes(dataLength), 0, bytes, 4, 2);
            bytes[6] = channel;
            bytes[7] = command;
            bytes[8] = byteCount;
            Array.Copy(values, 0, bytes, 9, values.Length);
            return bytes;
        }
        /// <summary>
        /// 回复读模拟量
        /// </summary>
        /// <param name="flag"></param>
        /// <param name="channel"></param>
        /// <param name="command"></param>
        /// <param name="registerCount"></param>
        /// <param name="values"></param>
        /// <returns></returns>
        private byte[] CreateReadAnalogyResponse(short flag, byte channel, byte command, byte registerCount, byte[] values)
        {
            byte[] bytes = new byte[7 + 2 + 2 * registerCount]; //回复字节长度，前面7个字节固定长度 + functionCode一个字节 + byteCount一个字节+registerCount*2个字节(一个寄存器占两个字节)
            Array.Copy(byteTransform.GetBytes(flag), 0, bytes, 0, 2);
            bytes[2] = 0x00;
            bytes[3] = 0x00;
            short dataLength = (short)(3 + 2 * registerCount);
            Array.Copy(byteTransform.GetBytes(dataLength), 0, bytes, 4, 2);
            bytes[6] = channel;
            bytes[7] = command;
            bytes[8] = (byte)(2 * registerCount);
            Array.Copy(values, 0, bytes, 9, values.Length);
            return bytes;
        }
        /// <summary>
        /// 错误回复
        /// </summary>
        /// <param name="flag"></param>
        /// <param name="channel"></param>
        /// <param name="command"></param>
        /// <param name="error"></param>
        /// <returns></returns>
        private byte[] CreateError(short flag, byte channel, byte command, byte error)
        {
            byte[] bytes = new byte[9];
            Array.Copy(byteTransform.GetBytes(flag), 0, bytes, 0, 2);
            bytes[2] = 0x00;
            bytes[3] = 0x00;
            bytes[4] = 0x00;
            bytes[5] = 0x03;
            bytes[6] = channel;
            bytes[7] = (byte)(command | 0x80);
            bytes[8] = error;
            return bytes;
        }
        /// <summary>
        /// 定时器
        /// </summary>
        /// <returns></returns>
        private bool OnTimer()
        {
            LeakTestSimulator();
            return true;
        }

        #region 模拟方法
        /// <summary>
        /// Loader LeakTest模拟
        /// </summary>
        private void LeakTestSimulator()
        {
            if (DOBytes[DONameIndexDic["c_VACUUM_TEST"]] == 1 && _festoDataBuffer["FlowTestClamp"] && AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] == 0)
            {
                AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] = 15000;
            }
            else if (DOBytes[DONameIndexDic["c_VACUUM_TEST"]] == 0 && !_festoDataBuffer["FlowTestClamp"])
            {
                AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] = 0;
            }
            if (DOBytes[DONameIndexDic["c_VACUUM_TEST"]] == 1 && _festoDataBuffer["FlowTestClamp"] && AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] > 3500)
            {
                AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] -= 40;
                if (AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] < 14000)
                {
                    AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] -= 140;
                }
            }
        }
        #endregion
    }
}