CyberX8/Framework/SimulatorCore/Commons/LinmotSerialPortDevice.cs

using Aitex.Core.RT.DataCenter;
using MECF.Framework.Common.Net;
using MECF.Framework.Simulator.Core.Driver;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Timers;

namespace MECF.Framework.Simulator.Core.Commons
{
    public class LinmotSerialPortDevice : BaseSerialSimulator
    {
        #region 常量
        private const string PORT_NAME = "com24";
        private byte[] _channels =new byte[] { 1, 2, 3, 4 };

        //状态字，13位-是否正在运动，11-home标识，10-是否到达目标位置，5-Quick Stop常为1，4-Power常为1，0-Operation enabled
        private Dictionary<int, short> _statusWordDictionary = new Dictionary<int, short>();
        private Dictionary<int, int> _currentPositionDictionary = new Dictionary<int, int>();
        IByteTransform _byteTransform;
        private Dictionary<int,int> _targetPositionDictionary = new Dictionary<int, int>();
        private Dictionary<int,int> _nextPositionDictionary = new Dictionary<int, int>();
        private Dictionary<int,int> _positionInterDictionary = new Dictionary<int, int>();
        private Dictionary<int,string> _directionDictionary = new Dictionary<int, string>();
        private Dictionary<int, System.Timers.Timer> _timerDictionary = new Dictionary<int, System.Timers.Timer>();
        private Dictionary<int, int> _speedDictionary = new Dictionary<int, int>();
        #endregion

        #region 内部变量

        #endregion
        public LinmotSerialPortDevice(string port) : base(port, SerialType.BUFFER)
        {
            _byteTransform = new SmallEndianByteTransformBase();
            _timerDictionary[1] = new System.Timers.Timer();
            _timerDictionary[1].Elapsed += Timer1_Elapsed;
            _timerDictionary[1].Interval = 100;
            _timerDictionary[1].Enabled = false;

            _timerDictionary[2] = new System.Timers.Timer();
            _timerDictionary[2].Elapsed += Timer2_Elapsed;
            _timerDictionary[2].Interval = 100;
            _timerDictionary[2].Enabled = false;

            _timerDictionary[3] = new System.Timers.Timer();
            _timerDictionary[3].Elapsed += Timer3_Elapsed;
            _timerDictionary[3].Interval = 100;
            _timerDictionary[3].Enabled = false;

            _timerDictionary[4] = new System.Timers.Timer();
            _timerDictionary[4].Elapsed += Timer4_Elapsed;
            _timerDictionary[4].Interval = 100;
            _timerDictionary[4].Enabled = false;

            for(int i=1;i<=4;i++)
            {
                _statusWordDictionary[i] = 0;
                _currentPositionDictionary[i] = 0;
                _directionDictionary[i] = "";
                _nextPositionDictionary[i] = 0;
                _positionInterDictionary[i] = 0;
                _targetPositionDictionary[i] = 0;
                _speedDictionary[i] = 0;
            }
        }

        private void Timer1_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            Elaspsed(1);
        }

        private void Timer2_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            Elaspsed(2);
        }

        private void Timer3_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            Elaspsed(3);
        }

        private void Timer4_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            Elaspsed(4);
        }

        private void Elaspsed(int index)
        {
            _currentPositionDictionary[index] += _positionInterDictionary[index];
            if ((_directionDictionary[index] == "down" && _currentPositionDictionary[index] <= _targetPositionDictionary[index]) ||
                _directionDictionary[index] == "up" && _currentPositionDictionary[index] >= _targetPositionDictionary[index])
            {
                if (_targetPositionDictionary[index] == _nextPositionDictionary[index])
                {
                    _statusWordDictionary[index] = 2103;
                    _timerDictionary[index].Stop();
                    return;
                }
                _targetPositionDictionary[index] = _nextPositionDictionary[index];
                if (_targetPositionDictionary[index] >= _currentPositionDictionary[index])
                {
                    _directionDictionary[index] = "up";
                }
                else
                {
                    _directionDictionary[index] = "down";
                }
                _positionInterDictionary[index] = (_targetPositionDictionary[index] - _currentPositionDictionary[index]) / 100;
            }
        }

        protected override void ProcessMessageBuffer(byte[] data)
        {
            Console.WriteLine($"Process {MessageConvert(data)}");
            if (data[0] != 0x01 && data[data.Length-1]!=0x04)
            {
                return;
            }
            int count = data.Length;
            byte[] tmp = new byte[data.Length];
            Array.Copy(data, 0, tmp, 0, data.Length);
            while(count>0)
            {
                var result = AnalyseData(tmp);
                if(result.startIndex!=-1&&result.endIndex!=-1)
                {
                    Console.WriteLine($"Process success {MessageConvert(data)}");
                    byte[] byt = new byte[result.endIndex - result.startIndex+1];
                    Array.Copy(tmp, result.startIndex, byt, 0, byt.Length);
                    AnalyseStandardByte(byt);
                    if(result.endIndex==tmp.Length-1)
                    {
                        break;
                    }
                    byte[] temp = new byte[tmp.Length - result.endIndex - 1];
                    Array.Copy(tmp, result.endIndex + 1, temp, 0, temp.Length);

                    tmp=new byte[temp.Length];
                    Array.Copy(temp,0,tmp, 0, temp.Length);
                }
                else
                {
                    Console.WriteLine($"Process fail {MessageConvert(data)}");
                    return;
                }
            }            
        }

        private void AnalyseStandardByte(byte[] data)
        {
            byte channel = data[1];
            if (!_channels.Contains(channel))
            {
                return;
            }
            if(data.Length<7)
            {
                return;
            }
            byte length = data[2];
            //指令
            if (data.Length >= 8)
            {
                if (data[4] == 0x00 && data[5] == 0x01)
                {
                    if (data[6] == 0x3E && data[7] == 0x00)//switch off
                    {
                        if (_statusWordDictionary[channel] >= 2048)
                        {
                            _statusWordDictionary[channel] = 2096;
                        }
                        else
                        {
                            _statusWordDictionary[channel] = 48;
                        }
                    }
                    else if (data[6] == 0x3F && data[7] == 0x00)//switch on
                    {
                        _statusWordDictionary[channel] = 2103;
                    }
                    else if (data[6] == 0x3F && data[7] == 0x20)//goto initial position
                    {
                        _statusWordDictionary[channel] = 11319;
                        _currentPositionDictionary[channel] = 50000;
                    }
                    else if (data[6] == 0x1F && data[7] == 0x00)//freeze
                    {
                        _statusWordDictionary[channel] = 2103;
                        _timerDictionary[channel].Stop();
                    }
                }
                else
                {
                    if (data[4] == 0x00 && data[5] == 0x02)
                    {
                        if (data[7] == 0x01)//VAI Go to Position
                        {
                            Console.WriteLine("VAI Go to Position");
                            _statusWordDictionary[channel] = 10295;
                            byte[] positionByt = new byte[4];
                            Array.Copy(data, 8, positionByt, 0, 4);
                            if (!_timerDictionary[channel].Enabled)
                            {
                                _targetPositionDictionary[channel] = BitConverter.ToInt32(positionByt, 0);
                                _positionInterDictionary[channel] = (_targetPositionDictionary[channel] - _currentPositionDictionary[channel]) / 100;
                                _timerDictionary[channel].Enabled = true;
                                if (_targetPositionDictionary[channel] < _currentPositionDictionary[channel])
                                {
                                    _directionDictionary[channel] = "down";
                                }
                                else
                                {
                                    _directionDictionary[channel] = "up";
                                }
                                _timerDictionary[channel].Start();
                            }
                            else
                            {
                                _nextPositionDictionary[channel] = BitConverter.ToInt32(positionByt, 0);
                            }
                        }
                        else if (data[7]==0x20)//Command Table 
                        {
                            _statusWordDictionary[channel] = 10295;
                        }
                    }
                    else if (data[4] == 0x01 && data[5]==0x03)//RAM 
                    {
                        if (data[6] == 0xCB && data[7]==0x14)
                        {
                            int value = _byteTransform.TransInt32(data, 8);
                            _speedDictionary[channel] = value;
                        }
                    }
                }
            }
            WriteBuffer(response(channel));
        }
        /// <summary>
        /// 解析
        /// </summary>
        /// <param name="byt"></param>
        /// <returns></returns>
        private (int startIndex,int endIndex) AnalyseData(byte[] byt)
        {
            int startIndex = -1;
            int endIndex = -1;
            int dataLength = 0;
            for (int i=0;i<byt.Length;i++)
            {
                if (byt[i]==0x01&&startIndex==-1)
                {
                    startIndex = i;
                    dataLength = byt[2];
                }

                if (startIndex != -1 && byt[i]==0x04)
                {
                    if (i == dataLength + 3)
                    {
                        endIndex = i;
                        break;
                    }
                }
            }
            return (startIndex, endIndex);
        }
        private byte[] response(byte channel)
        {
            byte[] byt = new byte[20];
            byt[0] = 0x01;
            byt[1] = channel;
            byt[2] = (byte)(byt.Length - 4);
            byt[3] = 0x02;
            byte[] statuswordByt = _byteTransform.GetBytes(_statusWordDictionary[channel]);
            byt[7] = statuswordByt[0];
            byt[8] = statuswordByt[1];
            byte[] positionByte = _byteTransform.GetBytes(_currentPositionDictionary[channel]);
            Array.Copy(positionByte, 0, byt, 15, positionByte.Length);
            byt[19] = 0x04;
            return byt;
        }
    }
}