CyberX8/CyberX8_Simulator/Devices/GalilSocketSimulator.cs

using Aitex.Common.Util;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Routine;
using Aitex.Core.Util;
using CyberX8_Core;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.Device.Festo;
using MECF.Framework.Common.Device.Galil;
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;
using System.Text;
using Xceed.Wpf.Toolkit.Panels;

namespace CyberX8_Simulator.Devices
{
    public class GalilSocketSimulator : SocketDeviceSimulator
    {
        #region 常量
        //最大运动轴数量（abcdefg）
        private const int MAX_AXIS_NUM = 8;
        //GalilControllerData最大数据长度
        private const int MAX_CONTROL_DATA_LENGTH = 264;
        //GalilAxisData最大数据长度
        private const int MAX_AXIS_DATA_LENGTH = 28;
        #endregion

        #region 内部变量
        private IByteTransform byteTransform = new BigEndianByteTransformBase();
        /// <summary>
        /// Galil数据
        /// </summary>
        private GalilControllerData _galilControlData = new GalilControllerData();
        /// <summary>
        /// Axis名称字典(key:axisName, value:index)
        /// </summary>
        private Dictionary<string, int> _axisNameIndexDic = new Dictionary<string, int>();
        /// <summary>
        /// ModuleName
        /// </summary>
        /// <param name="port"></param>
        private string _moduleName;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="port"></param>
        public GalilSocketSimulator(int port) :base(port)
        {
            InitData(port);
        }
        /// <summary>
        /// 解析信息
        /// </summary>
        /// <param name="data"></param>
        protected override void ProcessUnsplitMessage(byte[] data)
        {
            string cmdStr = ASCIIEncoding.ASCII.GetString(data);
            if (CheckCmdValid(cmdStr))
            {
                var cmdOperation = AnalyseCommand(cmdStr);
                byte[] response;
                if(cmdOperation.Item1 == "QR")
                {
                    //Read
                    response = CreateReadResponse(_galilControlData);
                }
                else
                {
                    //Write
                    SetOperation(cmdOperation.Item1, cmdOperation.Item2, cmdOperation.Item3);
                    response = CreateWriteResponse();
                }
                OnWriteMessage(response);
            }
            else
            {
                OnWriteMessage(CreateError());
            }                   
        }
        /// <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[] CreateReadResponse(GalilControllerData data)
        {
            //数据头(4 bytes)
            int headLength = 4;
            byte[] result = new byte[headLength + MAX_CONTROL_DATA_LENGTH];
            result[0] = 0x00;
            result[1] = 0x00;
            short dataLength = (short)(headLength + MAX_CONTROL_DATA_LENGTH);
            Array.Copy(byteTransform.GetBytes(dataLength), 0, result, 2, 2);
            //数据体(MAX_CONTROL_DATA_LENGTH bytes)
            Array.Copy(CodeControlData(data), 0, result, 4, MAX_CONTROL_DATA_LENGTH);
            return result;
        }
        /// <summary>
        /// 写回复
        /// </summary>
        /// <param name="flag"></param>
        /// <param name="channel"></param>
        /// <param name="command"></param>
        /// <param name="startAddress"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        private byte[] CreateWriteResponse()
        {
            //数据头(1 bytes)
            int headLength = 1;
            byte[] result = new byte[headLength];
            result[0] = 0x3A;
            return result;
        }
        /// <summary>
        /// 错误回复
        /// </summary>
        /// <param name="flag"></param>
        /// <param name="channel"></param>
        /// <param name="command"></param>
        /// <param name="error"></param>
        /// <returns></returns>
        private byte[] CreateError()
        {
            int headLength = 1;
            byte[] result = new byte[headLength];
            //Command not valid in program
            result[0] = 0x03;
            return result;
        }
        /// <summary>
        /// 初始化
        /// </summary>
        private void InitData(int port)
        {
            //初始化AxisData(最大MAX_AXIS_NUM个axis)
            _galilControlData.GalilAxisDatas = new List<GalilAxisData>();
            for (int i = 0; i < MAX_AXIS_NUM; i++)
            {
                _galilControlData.GalilAxisDatas.Add(new GalilAxisData());
                _galilControlData.GalilAxisDatas[i].Status = 0x01;
            }
            _galilControlData.Inputs  = new byte[10];
            _galilControlData.Outputs = new byte[10];   
            _galilControlData.SBlocks = new byte[8];   
            _galilControlData.TBlocks = new byte[8];
            //电机数据更新事件
            MotorSimulator.Instance.OnUpdateVariableValueChanged += UpdataRealTimeMotionData;
            //加载对应配置文件 GalilControllerCfg-Simulator.xml
            string oldXmlPath = PathManager.GetCfgDir();
            string newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Devices\\GalilControllerCfg-Simulator.xml";
            GalilControllerCfg cfg = CustomXmlSerializer.Deserialize<GalilControllerCfg>(new FileInfo(newXmlPath));
            foreach (GalilDeviceConfig config in cfg.GalilDeviceConfigs)
            {
                if (port == config.Port)
                {
                    _moduleName = config.Module;
                    foreach (GalilAxisConfig item in config.GalilAxises)
                    {
                        _axisNameIndexDic[$"{config.Module}.{item.Name}"] = item.Index;
                    }
                }
            }
        }
        /// <summary>
        /// GalilControllerData编码
        /// </summary>
        /// <returns></returns>
        private byte[] CodeControlData(GalilControllerData ctrlData)
        {
            byte[] result = new byte[MAX_CONTROL_DATA_LENGTH];
            int index = 0;
            //Sample(2 bytes)
            Array.Copy(byteTransform.GetBytes(ctrlData.Sample), 0, result, index, 2);
            index += 2;
            //Inputs(1*10 bytes)
            Array.Copy(ctrlData.Inputs, 0, result, index, 10);
            index += 10;
            //Outputs(1*10 bytes)
            Array.Copy(ctrlData.Outputs, 0, result, index, 10);
            index += 10;
            //Error Code(1 bytes)         
            result[13] = ctrlData.ErrorCode;
            index ++;
            //General Status(1 bytes)
            result[14] = ctrlData.Status;
            index ++;
            //S Block(2+2+4=8 bytes)
            Array.Copy(ctrlData.SBlocks, 0, result, 15, 8);
            index += 8;
            //T Block(2+2+4=8 bytes)
            Array.Copy(ctrlData.TBlocks, 0, result, 23, 8);
            index += 8;
            //Axis Datas(28 * 8 bytes)
            for(int i = 0;i < MAX_AXIS_NUM; i++)
            {
                Array.Copy(CodeAxisData(ctrlData.GalilAxisDatas[i]), 0, result, index, MAX_AXIS_DATA_LENGTH);
                index += MAX_AXIS_DATA_LENGTH;
            }     
            return result;
        }
        /// <summary>
        /// GalilAxisData编码
        /// </summary>
        /// <param name="axisData"></param>
        /// <returns></returns>
        private byte[] CodeAxisData(GalilAxisData axisData)
        {
            byte[] result = new byte[MAX_AXIS_DATA_LENGTH];
            int index = 0;
            //Status(2 bytes)
            Array.Copy(byteTransform.GetBytes(axisData.Status), 0, result, index, 2);
            index += 2;
            //Switches(1 bytes)
            result[index] = axisData.Switches;
            index ++;
            //Stop Code(1 bytes)
            result[index] = axisData.StopCode;
            index ++;
            //Reference Position(4 bytes)
            Array.Copy(byteTransform.GetBytes(axisData.ReferencePosition), 0, result, index, 4);
            index += 4;
            //Motor Position(4 bytes)
            Array.Copy(byteTransform.GetBytes(axisData.MotorPosition), 0, result, index, 4);
            index += 4;
            //Position Error
            Array.Copy(byteTransform.GetBytes(axisData.PositionError), 0, result, index, 4);
            index += 4;
            //Auxiliary Position
            Array.Copy(byteTransform.GetBytes(axisData.AuxiliaryPosition), 0, result, index, 4);
            index += 4;
            //Velocity
            Array.Copy(byteTransform.GetBytes(axisData.Velocity), 0, result, index, 4);
            index += 4;
            //Torque
            Array.Copy(byteTransform.GetBytes(axisData.Torque), 0, result, index, 2);
            index += 2;
            //Analog
            Array.Copy(byteTransform.GetBytes(axisData.Res), 0, result, index, 2);
            return result;
        }
        /// <summary>
        /// 解析指令(操作符, 运动轴, 操作数)
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        private (string, char, int) AnalyseCommand(string cmdStr)
        {
            var result = ("", ' ', -1);
            //操作符
            result.Item1 = cmdStr.Substring(0, 2);
            //运动轴
            if (cmdStr.Length >= 4) 
            {
                result.Item2 = Convert.ToChar(cmdStr.Substring(2, 1));
            }
            //操作数
            if (cmdStr.Length >= 5 && int.TryParse(cmdStr.Substring(4, cmdStr.Length - 5), out int tmp))
            {
               result.Item3 = tmp;      
            }
            return result;
        }
        /// <summary>
        /// CMD校验
        /// </summary>
        /// <returns></returns>
        private bool CheckCmdValid(string cmdStr)
        {
            //长度
            if(cmdStr.Length < 3) return false;
            //;结尾
            if (!cmdStr.EndsWith(";")) return false;
            //第1位为A~Z
            if (cmdStr[0] < 'A' || cmdStr[0] > 'Z') return false;
            //第2位为A~Z
            if (cmdStr[1] < 'A' || cmdStr[1] > 'Z') return false;
            if (cmdStr.Length >= 4)
            {
                //axis名称为A~H
                if (cmdStr[2] < 'A' || cmdStr[2] > 'H') return false;
                //=
                if(cmdStr.Length > 4 && !cmdStr.Contains("="))  return false;
            }
            
            return true;
        }
        /// <summary>
        /// 设置操作
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="axis"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        private bool SetOperation(string cmd, char axis, int value)
        {
            switch (cmd)
            {
                case "SH":
                    SwitchMotor(axis, true);
                    break;
                case "MO":
                    SwitchMotor(axis, false);
                    break;
                case "PR":
                    SetTargetRelativePosition(axis, value);
                    break;
                case "PA":
                    SetTargetAbsolutePosition(axis, value);
                    break;
                case "SP":
                    break;
                case "AC":
                    break;
                case "DC":
                    break;
                case "ST":
                    break;
                case "BG":
                    break;
                case "HM":
                    break;
                case "DP":
                    break;
                case "DE":
                    break;
                default:
                    break;
            }
            return true;
        }
        /// <summary>
        /// 实时更新电机数据
        /// </summary>
        /// <param name="datasDic"></param>
        private void UpdataRealTimeMotionData(Dictionary<string, CommandMotionData> datasDic)
        {
            foreach(var dataItem in datasDic)
            {
                if (_axisNameIndexDic.ContainsKey(dataItem.Key))
                {
                    UpdateAxisData(_axisNameIndexDic[dataItem.Key], dataItem.Value);
                }
            }
        }
        /// <summary>
        /// 更新对应Axis数据
        /// </summary>
        /// <param name="index"></param>
        private void UpdateAxisData(int index, CommandMotionData data)
        {
            _galilControlData.GalilAxisDatas[index].Velocity = (int)data.ActualVelocity;
            _galilControlData.GalilAxisDatas[index].MotorPosition = (int)data.MotorPosition;
            _galilControlData.GalilAxisDatas[index].ReferencePosition = (int)data.TargetPosition;
            _galilControlData.GalilAxisDatas[index].Torque = (short)data.ActualTorque;
        }
        #region AxisOperation
        /// <summary>
        /// 开/关电机
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="flag"></param>
        private void SwitchMotor(char axis, bool flag)
        {
            byte[] data = byteTransform.GetBytes(_galilControlData.GalilAxisDatas[axis - 'A'].Status);
            data[0] &= 0xFE;
            data[0] |= (byte)(flag ? 0x00 : 0x01);
            _galilControlData.GalilAxisDatas[axis - 'A'].Status = byteTransform.TransUInt16(data, 0);
        }
        /// <summary>
        /// 设置目标位置
        /// </summary>
        private void SetSpeed(char axis, int value)
        {
            _galilControlData.GalilAxisDatas[axis - 'A'].Velocity = value;
        }
        /// <summary>
        /// 设置目标绝对位置
        /// </summary>
        private void SetTargetAbsolutePosition(char axis, int value)
        {
            _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition = value;
        }
        /// <summary>
        /// 设置目标相对位置
        /// </summary>
        private void SetTargetRelativePosition(char axis, int value)
        {
            _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition += value;
        }
        #endregion
    }
}