JET-EFEM/athos/athosSimulator/Robot/RorzeRobot751Simulator.cs

using System;
using MECF.Framework.Simulator.Core.Driver;
using Aitex.Core.Util;
using System.Collections.Generic;
using System.Text.RegularExpressions;
using System.Threading;

namespace athosSimulator.Robot
{
    public class RorzeRobot751Simulator : SocketDeviceSimulator
    {
        public enum STAT_s1 
        { 
            OperationMode,
            OriginSearchCompletion,
            CommandProcessing,
            OperationStatus,
            OperationSpeed
        }
        public enum STAT_s2
        {
            IdentificationCode,
            ErrorCode
        }
        private string _motionCmd = "HOME,EXTD,LOAD,UNLD,TRNS,EXCH,CLMP,UCLM,WMAP,UTRN,MGET,MPUT,MGT1,MGT2,MPT1,MPT2,WCHK,ALEX,ALLD,ALUL,ALGT,ALEA,ALMV,STEP,GCHK";
        private const string RORZECMD = "^oTRB1";
        private readonly char[] delimiters = { ':', '.', '(', ',' ,')' };
        /// <summary>
        /// STAT s1 / s2
        /// </summary>
        private char[] s1 = new char[5] { '0', '0' ,'0', '0', '0' };
        private char[] s2 = new char[4] { '0', '0', '0', '0' };

        public RorzeRobot751Simulator(int port) : base(port, -1, "\r", ' ')
        {
           
        }
        /// <summary>
        /// Message处理
        /// </summary>
        /// <param name="str"></param>
        protected override void ProcessUnsplitMessage(string str)
        {
            // 处理Message
            OnProcessMessage(str);
        }

        private void OnProcessMessage(string str)
        {
            if (!Regex.IsMatch(str, RORZECMD)) return;
            // 发送ACK 表示收到
            string[] cmdStrs = str.Split(delimiters);
            string ack = "";
            if(cmdStrs.Length < 2) return;
            string cmd = cmdStrs[1];
            if (_motionCmd.Contains(cmdStrs[1])) cmd = "MOTIONCMD";
            switch (cmd)
            {
                case "STAT":
                    ack = "aTRB1." + $"{cmdStrs[1]}" + $":{GetSTATStr()}";
                    break;
                case "INIT":
                    OnWriteMessage($"aTRB1.{cmdStrs[1]}:");
                    Thread.Sleep((ushort)500);
                    SetState("s1", (int)STAT_s1.OperationMode, '1');//OperationMode: Remote
                    SetState("s1", (int)STAT_s1.OriginSearchCompletion, '1');//OriginSearchCompletion: Completed
                    ack = "eTRB1." + $"STAT" + $":{GetSTATStr()}";
                    Thread.Sleep((ushort)3000);
                    break;
                case "ORGN":
                    OnWriteMessage($"aTRB1.{cmdStrs[1]}:");
                    Thread.Sleep((ushort)500);
                    SetState("s1", (int)STAT_s1.OriginSearchCompletion, '1');//OriginSearchCompletion: Completed
                    SetState("s1", (int)STAT_s1.OperationStatus, '0');//OperationStatus: Stop
                    ack = "eTRB1." + $"STAT" + $":{GetSTATStr()}";
                    Thread.Sleep((ushort)3000);
                    break;
                case "MOTIONCMD":
                    OnWriteMessage($"aTRB1.{cmdStrs[1]}:");
                    Thread.Sleep((ushort)500);
                    SetState("s1", (int)STAT_s1.OriginSearchCompletion, '1');//OriginSearchCompletion: Completed
                    SetState("s1", (int)STAT_s1.OperationStatus, '0');//OperationStatus: Stop
                    ack = "eTRB1." + $"STAT" + $":{GetSTATStr()}";
                    Thread.Sleep((ushort)3000);
                    break;
                case "EXST":
                    ack = "aTRB1." + $"{cmdStrs[1]}" + $":0";
                    break;
                default:
                    ack = "aTRB1." + $"{cmdStrs[1]}" + ":";
                    break;
            }
            
            OnWriteMessage(ack);           
        }
        /// <summary>
        /// 设置STAT数据状态
        /// </summary>
        /// <param name="module"></param>
        /// <param name="bitNum"></param>
        /// <param name="value"></param>
        private void SetState(string module, int bitNum, char value)
        {
            switch (module)
            {
                case "s1":
                    s1[bitNum] = value;
                    break;
                case "s2":
                    s2[bitNum] = value;
                    break;
                default:
                    break;
            }
        }
        /// <summary>
        /// Get STAT String (s1 / s2)
        /// </summary>
        /// <returns></returns>
        private string GetSTATStr()
        {
            string s1Str = (s1[0] + "" + s1[1] + s1[2] + s1[3] + s1[4]);
            string s2Str = (s2[0] + "" + s2[1] + s1[2] + s2[3]);
            return $"{s1Str}/{s2Str}";
        }
        

    }
}