CyberX8/CyberX8_Simulator/Devices/GalilSocketSimulator.cs

using Aitex.Common.Util;
using Aitex.Core.Util;
using MECF.Framework.Common.Beckhoff.AxisProvider;
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;

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_21 = 264;
        private const int MAX_CONTROL_DATA_LENGTH_40 = 366;
       //GalilAxisData最大数据长度
       private const int MAX_AXIS_DATA_LENGTH_21 = 28;
        private const int MAX_AXIS_DATA_LENGTH_40 = 36;

        private const string TARGET_VELOCITY = "TargetVelocity";
        private const string TARGET_ACCEL = "TargetAcceleration";
        private const string TARGET_DECEL = "TargetDeceleration";
        private const string TARGET_POSITION = "TargetPosition";
        private const string SWITCH_SIGNAL = "SwitchSignal";
        private const string ACTUAL_POSITION = "ActualPosition";
        private const string AUXILIARY_POSITION = "AuxiliaryPosition";
        private const string HOMING_SIGNAL = "HomingSignal";
        private const string MOTION_SIGNAL = "MotionSignal";
        private const string STOP_SIGNAL = "StopSignal";
        private const double MOTOR_POSITION_FACTOR = 31.25;
        private const string GALIL_21 = "Galil21";
        private const string GALIL_40 = "Galil40";
       
        #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>
        /// Axis名称列表(index - axisName)
        /// </summary>
        private string[] _nameAxisList = new string[MAX_AXIS_NUM];
        /// <summary>
        /// ModuleName
        /// </summary>
        /// <param name="port"></param>
        private string _moduleName;
        /// <summary>
        /// Galil Type
        /// </summary>
        private string _galilType;
        /// <summary>
        /// GalilControllerData数据长度
        /// </summary>
        private int _controlDataLength;
        /// <summary>
        /// GalilAxisDatas数据长度
        /// </summary>
        private int _axisDataLength;
        /// <summary>
        /// MotorPosition比例系数字典(index - motorPositionRate)
        /// </summary>
        private Dictionary<string, double> _motorPositionRateDic = new Dictionary<string, double>();
        #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 + _controlDataLength];
            result[0] = 0x00;
            result[1] = 0x00;
            short dataLength = (short)(headLength + _controlDataLength);
            Array.Copy(byteTransform.GetBytes(dataLength), 0, result, 2, 2);
            //数据体(MAX_CONTROL_DATA_LENGTH bytes)
            switch (_galilType)
            {
                case GALIL_21:
                    Array.Copy(CodeControlData21(data), 0, result, 4, MAX_CONTROL_DATA_LENGTH_21);
                    break;
                case GALIL_40:
                    Array.Copy(CodeControlData40(data), 0, result, 4, MAX_CONTROL_DATA_LENGTH_40);
                    break;
                default:
                    break;
            }           
            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]; 
            
            
            //电机数据更新事件
            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;
                    _galilType = config.GalilType;                   
                    foreach (GalilAxisConfig item in config.GalilAxises)
                    {
                        _axisNameIndexDic[$"{config.Module}.{item.Name}"] = item.Index;
                        _nameAxisList[item.Index] = $"{config.Module}.{item.Name}";
                    }
                }
            }
            //加载AxisProviderCfg.xml
            oldXmlPath = PathManager.GetCfgDir();
            newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Devices\\AxisProviderCfg.xml";
            BeckhoffAxisProviderCfg axisProviderCfg = CustomXmlSerializer.Deserialize<BeckhoffAxisProviderCfg>(new FileInfo(newXmlPath));
            foreach(BeckhoffProviderAxis item in axisProviderCfg.Axes)
            {
                for (int i = 0; i < MAX_AXIS_NUM; i++)
                {
                    if (_nameAxisList[i] == item.Name)
                    {
                        _motorPositionRateDic[item.Name] = item.MotorPositionRate;
                    }
                }                   
            }

            if (_galilType == GALIL_40)
            {
                _controlDataLength = MAX_CONTROL_DATA_LENGTH_40;
                _axisDataLength = MAX_AXIS_DATA_LENGTH_40;
                _galilControlData.SBlocks = new byte[10];
                _galilControlData.TBlocks = new byte[10];
            }
            else
            {
                _controlDataLength = MAX_CONTROL_DATA_LENGTH_21;
                _axisDataLength = MAX_AXIS_DATA_LENGTH_21;
                _galilControlData.SBlocks = new byte[8];
                _galilControlData.TBlocks = new byte[8];
            }
            
        }
        #region Galil编码
        /// <summary>
        /// GalilControllerData编码(Galil21)
        /// </summary>
        /// <returns></returns>
        private byte[] CodeControlData21(GalilControllerData ctrlData)
        {
            byte[] result = new byte[MAX_CONTROL_DATA_LENGTH_21];
            int index = 0;
            //Sample(2 bytes)
            Array.Copy(BitConverter.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(CodeAxisData21(ctrlData.GalilAxisDatas[i]), 0, result, index, MAX_AXIS_DATA_LENGTH_21);
                index += MAX_AXIS_DATA_LENGTH_21;
            }
            return result;
        }
        /// <summary>
        /// GalilAxisData编码(Galil21)
        /// </summary>
        /// <param name="axisData"></param>
        /// <returns></returns>
        private byte[] CodeAxisData21(GalilAxisData axisData)
        {
            byte[] result = new byte[MAX_AXIS_DATA_LENGTH_21];
            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(BitConverter.GetBytes(axisData.ReferencePosition), 0, result, index, 4);
            index += 4;
            //Motor Position(4 bytes)
            Array.Copy(BitConverter.GetBytes(axisData.MotorPosition), 0, result, index, 4);
            index += 4;
            //Position Error
            Array.Copy(BitConverter.GetBytes(axisData.PositionError), 0, result, index, 4);
            index += 4;
            //Auxiliary Position
            Array.Copy(BitConverter.GetBytes(axisData.AuxiliaryPosition), 0, result, index, 4);
            index += 4;
            //Velocity
            Array.Copy(BitConverter.GetBytes(axisData.Velocity), 0, result, index, 4);
            index += 4;
            //Torque
            Array.Copy(BitConverter.GetBytes(axisData.Torque), 0, result, index, 2);
            index += 2;
            //Analog
            Array.Copy(BitConverter.GetBytes(axisData.Res), 0, result, index, 2);
            return result;
        }
        /// <summary>
        /// GalilControllerData编码(Galil40)
        /// </summary>
        /// <returns></returns>
        private byte[] CodeControlData40(GalilControllerData ctrlData)
        {
            byte[] result = new byte[MAX_CONTROL_DATA_LENGTH_40];
            int index = 0;
            //Sample(2 bytes)
            Array.Copy(BitConverter.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;
            //reservers
            byte[] reservers = new byte[16];
            Array.Copy(reservers, 0, result, index, 16);
            index += 16;
            //ethernet
            byte[] ethernet = new byte[8];
            Array.Copy(ethernet, 0, result, index, 8);
            index += 8;
            //ethernet
            //Error Code(1 bytes)         
            result[13] = ctrlData.ErrorCode;
            index++;
            //General Status(1 bytes)
            result[14] = ctrlData.Status;
            index++;
            //ampliferStatus
            byte[] ampliferStatus = new byte[4];
            Array.Copy(ampliferStatus, 0, result, index, 4);
            index += 4;
            //counterModel
            byte[] counterModel = new byte[6];
            Array.Copy(counterModel, 0, result, index, 6);
            index += 6;
            //S Block(10 bytes)
            Array.Copy(ctrlData.SBlocks, 0, result, index, 10);
            index += 10;
            //T Block(10 bytes)
            Array.Copy(ctrlData.TBlocks, 0, result, index, 10);
            index += 10;
            //Axis Datas(36 * 8 bytes)
            for (int i = 0; i < MAX_AXIS_NUM; i++)
            {
                Array.Copy(CodeAxisData40(ctrlData.GalilAxisDatas[i]), 0, result, index, MAX_AXIS_DATA_LENGTH_40);
                index += MAX_AXIS_DATA_LENGTH_40;
            }
            return result;
        }
        /// <summary>
        /// GalilAxisData编码(Galil40)
        /// </summary>
        /// <param name="axisData"></param>
        /// <returns></returns>
        private byte[] CodeAxisData40(GalilAxisData axisData)
        {
            byte[] result = new byte[MAX_AXIS_DATA_LENGTH_40];
            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(BitConverter.GetBytes(axisData.ReferencePosition), 0, result, index, 4);
            index += 4;
            //Motor Position(4 bytes)
            Array.Copy(BitConverter.GetBytes(axisData.MotorPosition), 0, result, index, 4);
            index += 4;
            //Position Error
            Array.Copy(BitConverter.GetBytes(axisData.PositionError), 0, result, index, 4);
            index += 4;
            //Auxiliary Position
            Array.Copy(BitConverter.GetBytes(axisData.AuxiliaryPosition), 0, result, index, 4);
            index += 4;
            //Velocity
            Array.Copy(BitConverter.GetBytes(axisData.Velocity), 0, result, index, 4);
            index += 4;
            //Torque
            Array.Copy(BitConverter.GetBytes(axisData.Torque), 0, result, index, 4);
            index += 4;
            //Analog
            Array.Copy(BitConverter.GetBytes(axisData.Res), 0, result, index, 2);
            index += 2;
            byte hallInput = 0x00;
            result[index] = hallInput;
            index ++;
            byte reserved = 0x00;
            result[index] = reserved;
            index ++;
            byte[] userVariables = new byte[4];
            Array.Copy(userVariables, 0, result, index, 4);
            return result;
        }
        #endregion

        /// <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"://速度
                    SetSpeed(axis, value);
                    break;
                case "AC"://加速度
                    SetAcceleration(axis, value);
                    break;
                case "DC"://减速度
                    SetDeceleration(axis, value);
                    break;
                case "ST"://停止
                    ControlMotion(axis, false);
                    break;
                case "BG"://启动
                    ControlMotion(axis, true);
                    break;
                case "HM"://回零
                    Home(axis);
                    break;
                case "DP"://设定motor position
                    SetMotorPosition(axis, value);
                    break;
                case "DE"://设定auxiliary position
                    SetAuxiliaryPosition(axis, value);
                    break;
                default:
                    break;
            }
            return true;
        }
        /// <summary>
        /// 实时更新电机数据
        /// </summary>
        /// <param name="datasDic"></param>
        private void UpdataRealTimeMotionData(Dictionary<string, SimulatorMotionData> 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, SimulatorMotionData data)
        {
            _galilControlData.GalilAxisDatas[index].Velocity = data.ActualVelocity;
            _galilControlData.GalilAxisDatas[index].MotorPosition = (int)(data.ActualPosition / _motorPositionRateDic[_nameAxisList[index]]);
            _galilControlData.GalilAxisDatas[index].ReferencePosition = data.TargetPosition;
            _galilControlData.GalilAxisDatas[index].Torque = data.ActualTorque;
            _galilControlData.GalilAxisDatas[index].StopCode = data.StopCode;
        }
        #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);
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], SWITCH_SIGNAL, flag);
        }
        /// <summary>
        /// 设置速度
        /// </summary>
        private void SetSpeed(char axis, int value)
        {
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], TARGET_VELOCITY, value);
        }
        /// <summary>
        /// 设置加速度
        /// </summary>
        private void SetAcceleration(char axis, int value)
        {
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], TARGET_ACCEL, value);
        }
        /// <summary>
        /// 设置减速度
        /// </summary>
        private void SetDeceleration(char axis, int value)
        {
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], TARGET_DECEL, value);
        }
        /// <summary>
        /// 设置目标绝对位置(设定位置)
        /// </summary>
        private void SetTargetAbsolutePosition(char axis, int value)
        {
            _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition = value;
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], TARGET_POSITION, value);
        }
        /// <summary>
        /// 设置目标相对位置(设定位置)
        /// </summary>
        private void SetTargetRelativePosition(char axis, int value)
        {
            _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition = _galilControlData.GalilAxisDatas[axis - 'A'].MotorPosition + value;
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], TARGET_POSITION, _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition);
        }
        /// <summary>
        /// 设置 Motor Position(实际位置)
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="value"></param>
        private void SetMotorPosition(char axis, int value)
        {
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], ACTUAL_POSITION, value);
        }
        /// <summary>
        /// 设置 Auxiliary Position
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="value"></param>
        private void SetAuxiliaryPosition(char axis, int value)
        {
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], AUXILIARY_POSITION, value);
        }
        /// <summary>
        /// 控制运动
        /// </summary>
        private void ControlMotion(char axis, bool value)
        {
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], value ? MOTION_SIGNAL : STOP_SIGNAL, value);
        }
        /// <summary>
        /// 电机回零
        /// </summary>
        /// <param name="axis"></param>
        private void Home(char axis)
        {
            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], HOMING_SIGNAL, true);
        }
        #endregion
    }
}