CyberX8/Framework/Common/Device/LinMot/LinMotSerialDevice.cs

using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using DocumentFormat.OpenXml.ExtendedProperties;
using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace MECF.Framework.Common.Device.LinMot
{
    public enum LinMotOperation
    {
        None = 0,
        SwitchOn = 1,
        SwitchOff = 2,
        Home = 3,
        Curve = 4,
        GoToInitialPosition = 5,
        CurveGotoInitialPosition = 6,
        Reset=7,
        ClearError=8,
        StartCurve=9,
        StopMotor=10,
        ControlWordGoToPosition=11,
        Freeze=12,
        StartVAIGoToPosition=13,
        ReadStatus=99
    }
    public class LinMotSerialDevice
    {
        #region 常量
        /// <summary>
        /// 开始标识
        /// </summary>
        private const byte START_FLAG = 0X01;
        /// <summary>
        /// 结束标识
        /// </summary>
        private const byte END_FLAG = 0x04;
        /// <summary>
        /// 最小长度
        /// </summary>
        private const byte MIN_LENGTH = 5;
        /// <summary>
        /// 最大长度(1(Header)+1(ID)+1(Length)+3(StartData+Msg High+Msg Low)+n(Data最大63)+1(End))
        /// </summary>
        private const byte MAX_LENGTH = 70;
        /// <summary>
        /// 缓存最大
        /// </summary>
        private const int BUFFER_LENGTH = 65535;
        #endregion

        #region 内部变量
        /// <summary>
        /// 串口
        /// </summary>
        private SerialPort _serialPort;
        /// <summary>
        /// 缓存
        /// </summary>
        private byte[] _buffer = new byte[BUFFER_LENGTH];
        /// <summary>
        /// 缓存锁
        /// </summary>
        private object _lock = new object();
        /// <summary>
        /// 偏移量
        /// </summary>
        private int offset = 0;
        /// <summary>
        /// 数量
        /// </summary>
        private int count = 0;
        /// <summary>
        /// 定时任务
        /// </summary>
        private PeriodicJob _periodJob;
        /// <summary>
        /// 发送定时任务
        /// </summary>
        private PeriodicJob _sendPeriodJob;
        /// <summary>
        /// 连接状态
        /// </summary>
        private bool _connected;
        /// <summary>
        /// 发送队列
        /// </summary>
        private ConcurrentQueue<byte[]> _sendOperationQueue = new ConcurrentQueue<byte[]>();
        /// <summary>
        /// 模块名称
        /// </summary>
        private string _name;
        /// <summary>
        /// 支持重连
        /// </summary>
        private bool _reconnect;
        /// <summary>
        /// 错误
        /// </summary>
        private string _errmsg;
        /// <summary>
        /// 离线时间
        /// </summary>
        private DateTime _offlineDateTime = DateTime.Now;
        /// <summary>
        /// 首次连接成功
        /// </summary>
        private bool _isFirstConnected = false;
        /// <summary>
        /// 自增锁
        /// </summary>
        private object _autoCountLock = new object();
        /// <summary>
        /// 自动增加计数
        /// </summary>
        private byte _autoCount = 0;
        #endregion

        #region 属性
        /// <summary>
        /// 连接状态
        /// </summary>
        public bool Connected
        {
            get { return _connected; }
            set { _connected = value; }
        }
        #endregion
        /// <summary>
        /// 初始化
        /// </summary>
        /// <param name="portName"></param>
        /// <param name="baudRate"></param>
        /// <param name="stopBits"></param>
        /// <param name="dataBits"></param>
        /// <param name="parity"></param>
        public LinMotSerialDevice(string name, string portName, int baudRate = 9600, StopBits stopBits = StopBits.One, int dataBits = 8, Parity parity = Parity.None, bool reconnect = false)
        {
            _serialPort = new SerialPort();
            _serialPort.BaudRate = baudRate;
            _serialPort.StopBits = stopBits;
            _serialPort.DataBits = dataBits;
            _serialPort.Parity = parity;
            _serialPort.PortName = portName;
            _serialPort.DataReceived += SerialPort_DataReceived;
            _serialPort.ErrorReceived += SerialPort_ErrorReceived;
            _name = name;
            _reconnect = reconnect;
            _periodJob = new PeriodicJob(50, OnTimer, $"{_name}_Analyser", false, true);
            _sendPeriodJob = new PeriodicJob(50, OnSenderTimer, $"{_name}_Sender", false, true);
        }
        /// <summary>
        /// 初始化
        /// </summary>
        public void Initialize()
        {
            _periodJob.Start(); 
            _sendPeriodJob.Start();
        }
        /// <summary>
        /// 启动
        /// </summary>
        public void Start()
        {
            if (!_connected)
            {
                try
                {
                    _serialPort.Open();
                    LOG.WriteLog(eEvent.INFO_LINMOT, _name, $"connect port[{_serialPort.PortName}] success");
                    _connected = true;
                }
                catch (Exception ex)
                {
                    WriteErrorMsg(ex.Message);
                }

                if (!_isFirstConnected)
                {
                    _isFirstConnected = true;
                }
            }
        }
        /// <summary>
        /// 关闭
        /// </summary>
        public void Close()
        {
            try
            {
                _connected = false;
                _serialPort.Close();
                _periodJob.Stop();
            }
            catch (Exception ex)
            {
                WriteErrorMsg(ex.Message);
            }
        }
        /// <summary>
        /// 定时器
        /// </summary>
        /// <returns></returns>
        private bool OnTimer()
        {
            lock (_lock)
            {
                if (count < MIN_LENGTH)
                {
                    return true;
                }
            }
            try
            {
                var result = CheckValid();
                if (result.Item3)
                {
                    lock (_lock)
                    {
                        int startIndex = result.Item1;
                        int endIndex = result.Item2;
                        byte[] byt = new byte[endIndex - startIndex + 1];
                        Array.Copy(_buffer, startIndex, byt, 0, byt.Length);
                        Array.Copy(_buffer, endIndex + 1, _buffer, 0, count - endIndex - 1);
                        count = count - endIndex - 1;
                        offset = count;
                        _buffer[count] = 0;
                        _buffer[count + 1] = 0;
                        _buffer[count + 2] = 0;
                        _buffer[count + 3] = 0;
                        Analyse(byt);
                    }
                }
                else
                {
                    lock (_lock)
                    {
                        int startIndex = result.Item1;
                        int endIndex = result.Item2;
                        if (startIndex == -1)
                        {
                            _buffer = new byte[BUFFER_LENGTH];
                            offset = 0;
                            count = 0;
                        }
                        else if (startIndex > 0)
                        {
                            Array.Copy(_buffer, startIndex, _buffer, 0, count - startIndex);
                            count = count - startIndex;
                            offset = count;
                        }
                        else if (endIndex == -1 && count > MAX_LENGTH)//一直找不到结束标识，长度超过最大长度
                        {
                            //仅清除第一个StartFlag
                            Array.Copy(_buffer, 1, _buffer, 0, count - 1);
                            count = count - 1;
                            offset = count;
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, _name, ex.StackTrace);
            }
            return true;
        }
        /// <summary>
        /// 发送定时器
        /// </summary>
        /// <returns></returns>
        private bool OnSenderTimer()
        {
            if(!_isFirstConnected)
            {
                return true;
            }
            if (!_connected)
            {
                if (_sendOperationQueue.Count != 0)
                {
                    ClearSendQueue();
                }
                if (_reconnect)
                {
                    Start();
                }
                else
                {
                    _periodJob.Stop();
                }
                return true;
            }
            else
            {
                int queueCount = _sendOperationQueue.Count;
                if (_sendOperationQueue.Count != 0)
                {
                    WriteInfoMsg(1, $"start send buffer queue length {queueCount}");

                    if (_sendOperationQueue.TryDequeue(out byte[] sendBuffer))
                    {
                        try
                        {
                            WriteInfoMsg(1, $"start send buffer get queue data");
                            _serialPort.Write(sendBuffer, 0, sendBuffer.Length);
                            WriteInfoMsg(1, sendBuffer);
                            WriteInfoMsg(1, $"send buffer queue length {queueCount}");
                        }
                        catch (Exception ex)
                        {
                            WriteErrorMsg(ex.Message);
                        }
                    }
                }
            }
            return true;
        }


        /// <summary>
        /// 清空发送队列
        /// </summary>
        private void ClearSendQueue()
        {
            try
            {
                while(_sendOperationQueue.Count!=0)
                {
                    _sendOperationQueue.TryDequeue(out var result);
                }
            }
            catch
            {

            }
        }
        /// <summary>
        /// 校验
        /// </summary>
        /// <returns></returns>
        private (int, int, bool) CheckValid()
        {
            int startIndex = -1;
            int endIndex = -1;
            bool result = false;
            if(count<MIN_LENGTH)
            {
                return (-1, -1, false);
            }
            int dataLength = 0;
            for (int i = 0; i < count; i++)
            {
                if (_buffer[i] == START_FLAG && startIndex == -1)
                {
                    startIndex = i;
                    dataLength = _buffer[2];
                }
                else if (_buffer[i] == END_FLAG)
                {
                    if (i == dataLength + 3)
                    {
                        endIndex = i;
                        break;
                    }
                }
            }
            if (endIndex > startIndex)
            {
                result = true;
            }
            return (startIndex, endIndex, result);
        }
        /// <summary>
        /// 解析标准包
        /// </summary>
        /// <param name="byt"></param>
        private void Analyse(byte[] byt)
        {
            if (byt.Length < 19)
            {
                return;
            }
            byte id = byt[1];//Id
            short statusWord = BitConverter.ToInt16(byt,7);//7状态字高位 8状态字低位
            short errorCode=BitConverter.ToInt16(byt,11);//11错误码高位 12错误码低位
            int currentPosition = BitConverter.ToInt32(byt, 15);
            string _linmotDeviceModuleName = LinMotDeviceConfigManager.Instance.GetModuleNameByAddress(_name, id);
            if (!string.IsNullOrEmpty(_linmotDeviceModuleName))
            {
                LinMotDeviceConfigManager.Instance.UpdateModuleData(_linmotDeviceModuleName, statusWord, errorCode, currentPosition);
            }
        }
        /// <summary>
        /// 出现错误
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void SerialPort_ErrorReceived(object sender, SerialErrorReceivedEventArgs e)
        {
            LOG.WriteLog(eEvent.ERR_LINMOT, _name, e.EventType.ToString());
        }
        /// <summary>
        /// 串口接收
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void SerialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            byte[] readBuffer = new byte[_serialPort.BytesToRead];
            int readCount = _serialPort.Read(readBuffer, 0, readBuffer.Length);

            if (readCount == 0)
            {
                return;
            }

            WriteInfoMsg(0,readBuffer);

            lock (_lock)
            {
                Buffer.BlockCopy(readBuffer, 0, _buffer, offset, readCount);
                offset = offset + readCount;
                count = offset;
            }
        }
        /// <summary>
        /// 发送指令
        /// </summary>
        /// <param name="id"></param>
        /// <param name="operation"></param>
        public bool SendOperation(byte id,LinMotOperation operation)
        {
            if (_connected)
            {
                byte[] bytes = GenerateOperationBytes(id, operation);
                _sendOperationQueue.Enqueue(bytes);
                return true;
            }
            else
            {
                WriteErrorMsg($"{_name} is not connected");
                return false;
            }
        }
        /// <summary>
        /// 发送Curve指令
        /// </summary>
        /// <param name="id"></param>
        /// <param name="curveCount"></param>
        /// <param name="curveId"></param>
        /// <param name="timeScale"></param>
        /// <param name="curveOffset"></param>
        /// <param name="amplitudeScale"></param>
        public bool SendCurveOperation(byte id,ushort curveId,ushort timeScale,int curveOffset=0,short amplitudeScale=100)
        {
            if (_connected)
            {
                byte[] curveIdByt = BitConverter.GetBytes(curveId);

                byte[] curveOffsetByte = BitConverter.GetBytes(curveOffset);

                //timeScal--unit 0.01%
                byte[] timeScaleByt = BitConverter.GetBytes((ushort)(timeScale * 100));
                //amplitudeScale--unit 0.1%
                byte[] amplitudeScaleByte = BitConverter.GetBytes((short)(amplitudeScale * 10));

                GenerateAutoCount();
                byte[] byt = new byte[] { 0x01, id, 0x0F, 0x02, 0x00, 0x02, (byte)(0x40 | _autoCount), 0x04, curveIdByt[0], curveIdByt[1],curveOffsetByte[0], curveOffsetByte[1],
                curveOffsetByte[2], curveOffsetByte[3], timeScaleByt[0], timeScaleByt[1], amplitudeScaleByte[0], amplitudeScaleByte[1], 0x04 };
                _sendOperationQueue.Enqueue(byt);
                string str = string.Join(" ", Array.ConvertAll(byt, x => x.ToString("X2")));
                LOG.WriteBackgroundLog(eEvent.INFO_LINMOT, _name, $"curve operation {str}");
                return true;
            }
            else
            {
                WriteErrorMsg($"{_name} is not connected");
                return false;
            }
        }
        /// <summary>
        /// 发送CommandTable
        /// </summary>
        /// <param name="id"></param>
        /// <param name="curveCount"></param>
        /// <param name="entryId"></param>
        /// <returns></returns>
        public bool SendCommandTableOpertaion(byte id,  short entryId=0)
        {
            if (_connected)
            {
                byte[] entryIdByt = BitConverter.GetBytes(entryId);
                GenerateAutoCount();
                byte[] byt = new byte[] { 0x01, id, 0x07, 0x02, 0x00, 0x02, (byte)(0x00 | _autoCount), 0x20, entryIdByt[0], entryIdByt[1], 0x04 };
                _sendOperationQueue.Enqueue(byt);
                string str = string.Join(" ", Array.ConvertAll(byt, x => x.ToString("X2")));
                LOG.WriteBackgroundLog(eEvent.INFO_LINMOT, _name, $"command table {str}");
                return true;
            }
            else
            {
                WriteErrorMsg($"{_name} is not connected");
                return false;
            }
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="id"></param>
        /// <param name="lowByte"></param>
        /// <param name="highByte"></param>
        /// <param name="intValue"></param>
        /// <returns></returns>
        public bool SendWriteRamIntValue(byte id,byte lowByte,byte highByte,int intValue)
        {
            if (_connected)
            {
                byte[] curveOffsetByte = BitConverter.GetBytes(intValue);
                byte[] byt = new byte[] { 0x01, id, 0x09, 0x02, 0x01, 0x03, lowByte, highByte, curveOffsetByte[0], curveOffsetByte[1], curveOffsetByte[2], curveOffsetByte[3],0x04 };
                _sendOperationQueue.Enqueue(byt);
                string str = string.Join(" ", Array.ConvertAll(byt, x => x.ToString("X2")));
                LOG.WriteBackgroundLog(eEvent.INFO_LINMOT, _name, $"Write Ram Int {str}");
                return true;
            }
            else
            {
                WriteErrorMsg($"{_name} is not connected");
                return false;
            }
        }
        /// <summary>
        /// 发送GAI GoToPosition指令
        /// </summary>
        /// <param name="id"></param>
        /// <param name="autoCount"></param>
        /// <param name="curveId"></param>
        /// <param name="timeScale"></param>
        /// <param name="curveOffset"></param>
        /// <param name="amplitudeScale"></param>
        public bool SendVAIGoToPositionOpertaion(byte id, int position,int velocity,int accel,int decel)
        {
            if (_connected)
            {
                //int positionValue = (int)Math.Round(position * 10000, 0);
                byte[] positionByt = BitConverter.GetBytes(position);

                //unit 0.1μm  1mm=1000μm
                //int velocityValue = (int)Math.Round(velocity * 1000);
                byte[] velocityByt = BitConverter.GetBytes(velocity);

                //accel--unit 0.1m/s^2
                //byte[] accelByt = BitConverter.GetBytes(accel * 1000);
                byte[] accelByt = BitConverter.GetBytes(accel);
                //decel--unit 0.1m/s^2
                //byte[] decelByte = BitConverter.GetBytes(decel * 1000);
                byte[] decelByte = BitConverter.GetBytes(decel);
                GenerateAutoCount();
                byte[] byt = new byte[] { 0x01, id, 0x15, 0x02, 0x00, 0x02, (byte)(0x00 | _autoCount), 0x01, positionByt[0], positionByt[1], positionByt[2], positionByt[3],
                    velocityByt[0],velocityByt[1],velocityByt[2],velocityByt[3],accelByt[0],accelByt[1],accelByt[2],accelByt[3],
                    decelByte[0],decelByte[1],decelByte[2],decelByte[3],0x04 };
                _sendOperationQueue.Enqueue(byt);
                return true;
            }
            else
            {
                WriteErrorMsg($"{_name} is not connected");
                return false;
            }
        }
        /// <summary>
        /// 发送GAI GoToPosition After Actual Position指令
        /// </summary>
        /// <param name="id"></param>
        /// <param name="autoCount"></param>
        /// <param name="curveId"></param>
        /// <param name="timeScale"></param>
        /// <param name="curveOffset"></param>
        /// <param name="amplitudeScale"></param>
        public bool SendVAIGoToPositionAfterAutalCommandOpertaion(byte id, int position, int velocity, int accel, int decel)
        {
            if (_connected)
            {
                //int positionValue = (int)Math.Round(position * 10000, 0);
                byte[] positionByt = BitConverter.GetBytes(position);

                //unit 0.1μm  1mm=1000μm
                //int velocityValue = (int)Math.Round(velocity * 1000);
                byte[] velocityByt = BitConverter.GetBytes(velocity);

                //accel--unit 0.1m/s^2
                //byte[] accelByt = BitConverter.GetBytes(accel * 1000);
                byte[] accelByt = BitConverter.GetBytes(accel);
                //decel--unit 0.1m/s^2
                //byte[] decelByte = BitConverter.GetBytes(decel * 1000);
                byte[] decelByte = BitConverter.GetBytes(decel);
                GenerateAutoCount();
                byte[] byt = new byte[] { 0x01, id, 0x15, 0x02, 0x00, 0x02, (byte)(0x80 | _autoCount), 0x01, positionByt[0], positionByt[1], positionByt[2], positionByt[3],
                    velocityByt[0],velocityByt[1],velocityByt[2],velocityByt[3],accelByt[0],accelByt[1],accelByt[2],accelByt[3],
                    decelByte[0],decelByte[1],decelByte[2],decelByte[3],0x04 };
                _sendOperationQueue.Enqueue(byt);
                return true;
            }
            else
            {
                WriteErrorMsg($"{_name} is not connected");
                return false;
            }
        }

        /// <summary>
        /// 创建操作指令
        /// </summary>
        /// <param name="id"></param>
        /// <param name="operation"></param>
        /// <returns></returns>
        private byte[] GenerateOperationBytes(byte id,LinMotOperation operation)
        {
            switch(operation)
            {
                case LinMotOperation.ReadStatus:
                    return new byte[] { 0x01, id, 0x03, 0x02, 0x01, 0x00, 0x04 };
                case LinMotOperation.SwitchOn:
                    return new byte[] { 0x01, id, 0x05, 0x02, 0x00, 0x01, 0x3F, 0x00, 0x04 };
                case LinMotOperation.SwitchOff:
                    return new byte[] { 0x01, id, 0x05, 0x02, 0x00, 0x01, 0x3E, 0x00, 0x04 };
                case LinMotOperation.Home:
                    return new byte[] { 0x01, id, 0x05, 0x02, 0x00, 0x01, 0x3F, 0x08, 0x04 };
                case LinMotOperation.ClearError:
                    return new byte[] { 0x01, id, 0x05, 0x02, 0x00, 0x01, 0xBF, 0x00, 0x04 };
                case LinMotOperation.GoToInitialPosition:
                    return new byte[]{ 0x01, id, 0x05, 0x02, 0x00, 0x01, 0x3F, 0x20, 0x04 };
                case LinMotOperation.ControlWordGoToPosition:
                    return new byte[] { 0x01, id, 0x05, 0x02, 0x00, 0x01, 0x7F, 0x00, 0x04 };
                case LinMotOperation.Freeze:
                    return new byte[] { 0x01, id, 0x05, 0x02, 0x00, 0x01, 0x1F, 0x00, 0x04 };
                default:
                    return new byte[] { 0x01, id, 0x03, 0x02, 0x01, 0x00, 0x04 };
            }
        }

        /// <summary>
        /// 记录错误信息
        /// </summary>
        /// <param name="msg"></param>
        private void WriteErrorMsg(string msg, bool disConnected = true)
        {
            if (disConnected)
            {
                _connected = false;
                _offlineDateTime = DateTime.Now;
            }
            if (_errmsg != msg)
            {
                _errmsg = msg;
                LOG.WriteLog(eEvent.ERR_LINMOT, _name, msg);
            }
        }
        /// <summary>
        /// 写日志
        /// </summary>
        /// <param name="bytes"></param>
        private void WriteInfoMsg(int logType,byte[] bytes)
        {
            bool enableLog = false;
            if (SC.ContainsItem("Log.EnableLinmotLog"))
            {
                enableLog = SC.GetValue<bool>("Log.EnableLinmotLog");
            }
            if (enableLog)
            {
                string str = string.Join(" ", Array.ConvertAll(bytes, x => x.ToString("X2")));
                string type = logType == 0 ? "receive" : "send";
                LOG.WriteBackgroundLog(eEvent.INFO_LINMOT, _name, $"{type} {str}");
            }
        }
        /// <summary>
        /// 写日志
        /// </summary>
        /// <param name="bytes"></param>
        private void WriteInfoMsg(int logType, string str)
        {
            bool enableLog = false;
            if (SC.ContainsItem("Log.EnableLinmotLog"))
            {
                enableLog = SC.GetValue<bool>("Log.EnableLinmotLog");
            }
            if (enableLog)
            {
                string type = logType == 0 ? "receive" : "send";
                LOG.WriteBackgroundLog(eEvent.INFO_LINMOT, _name, $"{type} {str}");
            }
        }


        /// <summary>
        /// Curve每次数值得不同
        /// </summary>
        private void GenerateAutoCount()
        {
            lock (_autoCountLock)
            {
                _autoCount++;
                if (_autoCount >= 0x0D)
                {
                    _autoCount = 1;
                }
            }
        }
    }
}