CyberX8/Framework/SimulatorCore/Robots/YaskawaSR100ControllerSimulator.cs

using System;
using System.Threading;
using MECF.Framework.Simulator.Core.Driver;

namespace MECF.Framework.Simulator.Core.Robots
{

    public class YaskawaSR100ControllerSimulator : SocketDeviceSimulator
    {
        protected Random _rd = new Random();
 
        public YaskawaSR100ControllerSimulator(int port)
            :base(port, 3, "\r", ',')
        {
            //Action Command
            AddCommandHandler("INIT", HandleINIT);
            AddCommandHandler("MREL", HandleMREL);
            AddCommandHandler("MALN", HandleMALN);
            AddCommandHandler("MACA", HandleMACA);

            AddCommandHandler("MPNT", HandleMPNT);
            AddCommandHandler("MTRS", HandleMTRS);
            AddCommandHandler("MCTR", HandleMCTR);
            AddCommandHandler("MTCH", HandleMTCH);
            AddCommandHandler("MABS", HandleMABS);
            AddCommandHandler("MMAP", HandleMMAP);
            AddCommandHandler("MMCA", HandleMMCA);
 
            //Control Command
            AddCommandHandler("CSTP", HandleCSTP);
            AddCommandHandler("CRSM", HandleCRSM);
            AddCommandHandler("CSRV", HandleCSRV);
            AddCommandHandler("CCLR", HandleCCLR);
            AddCommandHandler("CSOL", HandleCSOL);

            //Setting Command
            AddCommandHandler("SSPD", HandleSSPD);
            AddCommandHandler("SSLV", HandleSSLV);
            AddCommandHandler("SPRM", HandleSPRM);
            AddCommandHandler("SMSK", HandleSMSK);

            //Reference Command
            AddCommandHandler("RSPD", HandleRSPD);
            AddCommandHandler("RSLV", HandleRSLV);
            AddCommandHandler("RPRM", HandleRPRM);
            AddCommandHandler("RSTS", HandleRSTS);
            AddCommandHandler("RERR", HandleRERR);
            AddCommandHandler("RMSK", HandleRMSK);
            AddCommandHandler("RVER", HandleRVER);
            AddCommandHandler("RALN", HandleRALN);
            AddCommandHandler("RACA", HandleRACA);
            AddCommandHandler("RCCD", HandleRCCD);
            AddCommandHandler("RLOG", HandleRLOG);
            AddCommandHandler("RMAP", HandleRMAP);

            //ACK
            AddCommandHandler("ACKN", HandleACKN);

            AddCommandHandler("Unknown", HandleUnknown);
        }

        public virtual void HandleRMAP(string obj)
        {
            Response("RMAP", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleMMCA(string obj)
        {
            Response("MMCA", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleMMAP(string obj)
        {
            Response("MMAP", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleMABS(string obj)
        {
            Response("MABS", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleMTCH(string obj)
        {
            Response("MTCH", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleMCTR(string obj)
        {
            Response("MCTR", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleMTRS(string obj)
        {
            Response("MTRS", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleMPNT(string obj)
        {
            Response("MPNT", obj);
            EndOfExecution(obj);
        }

        public virtual void HandleUnknown(string obj)
        {
            Response("", obj);
        }

        public virtual void HandleCSOL(string obj)
        {
            Response("CSOL", obj);
            EndOfExecution(obj);
        }

        private void HandleACKN(string obj)
        {
            //DO NOTHING
        }

        private void HandleRLOG(string obj)
        {
            Response("RLOG", obj);
        }

        private void HandleRCCD(string obj)
        {
            Response("RCCD", obj);
        }

        private void HandleRACA(string obj)
        {
            Response("RACA", obj);
        }

        private void HandleRALN(string obj)
        {
            Response("RALN", obj);
        }

        private void HandleRVER(string obj)
        {
            Response("RVER", obj);
        }

        private void HandleRMSK(string obj)
        {
            Response("RMSK", obj);
        }

        private void HandleRERR(string obj)
        {
            Response("RERR", obj);
        }


        //$,<UNo>(,<SeqNo>),RSTS(,<Sum>)<CR>
        //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,RSTS,<Errcd>,<Status>(,<Sum>)<CR>
        public virtual void HandleRSTS(string msg)
        {
            string[] arrayMsg = msg.Split(',');
            string unit = arrayMsg[1];
            string seq = arrayMsg[2];
            string cmd = arrayMsg[3];
            string sts = 0.ToString("X02");
            string Ackcd = 0.ToString("X04");
            string error = 0.ToString("X04");
            int status1 = Convert.ToInt32("0011", 2);
            string status = string.Format("{0}000", status1.ToString("X1"));

            string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, Ackcd, cmd, error, status);
            string sum = CheckSum(feedback);
            string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);

            OnWriteMessage(result);
        }

        private void HandleRPRM(string obj)
        {
            Response("RPRM", obj);
        }

        private void HandleRSLV(string obj)
        {
            Response("RSLV", obj);
        }

        private void HandleRSPD(string obj)
        {
            Response("RSPD", obj);
        }

        private void HandleSMSK(string obj)
        {
            Response("SMSK", obj);
        }

        private void HandleSPRM(string obj)
        {
            Response("SPRM", obj);
        }

        private void HandleSSLV(string obj)
        {
            Response("SSLV", obj);
        }

        private void HandleSSPD(string obj)
        {
            Response("SSPD", obj);
        }
        

        //$,<UNo>(,<SeqNo>),CCLR,<CMode>(,<Sum>)<CR>
        //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,CCLR,<CMode>(,<Sum>)<CR>
        //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,CCLR,<ExeTime>,<PosData1>…,<PosDataN>(,<Sum>)<CR>
        public  virtual void HandleCCLR(string msg)
        {
            string[] arrayMsg = msg.Split(',');
            string unit = arrayMsg[1];
            string seq = arrayMsg[2];
            string cmd = arrayMsg[3];
            string sts = 0.ToString("X02");
            string Ackcd = 0.ToString("X04");
            string cmode = arrayMsg[4];

            string feedback = string.Format(",{0},{1},{2},{3},{4},{5},", unit, seq, sts, Ackcd, cmd, cmode);
            string sum = CheckSum(feedback);
            string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);

            OnWriteMessage(result);

            Thread.Sleep(3000);

            int executeTime = _rd.Next(10000, 999999);
            string exeTime = executeTime.ToString("D6");
            string posData = GetPosData();
            string error = 0.ToString("X04");

            feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, cmd, exeTime, posData);
            sum = CheckSum(feedback);
            result = String.Format("{0}{1}{2}", "!", feedback, sum);

            OnWriteMessage(result);
        }

        private void HandleCSRV(string obj)
        {
            Response("CSRV", obj);
            EndOfExecution(obj);
        }

        private void HandleCRSM(string obj)
        {
            Response("CRSM", obj);
            EndOfExecution(obj);
        }

        private void HandleCSTP(string obj)
        {
            Response("CSTP", obj);
            EndOfExecution(obj);
        }

        private void HandleMACA(string obj)
        {
            Response("MACA", obj);
            EndOfExecution(obj);
        }


        //$,<UNo>(,<SeqNo>),MALN,<Mode>,<Angle>(,<Sum>)<CR>
        //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,MALN,<Mode>,<Angle>(,<Sum>)<CR>
        //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,MALN,<ExeTime>,<PosData1>…,<PosDataN>,<Value1>…,<Value10>(,<Sum>)<CR>
        public virtual void HandleMALN(string msg)
        {
            string[] arrayMsg = msg.Split(',');
            string unit = arrayMsg[1];
            string seq = arrayMsg[2];
            string cmd = arrayMsg[3];
            string sts = 0.ToString("X02");
            string Ackcd = 0.ToString("X04");
            string mode = arrayMsg[4];
            string angle = arrayMsg[5];

            string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, Ackcd, cmd, mode, angle);
            string sum = CheckSum(feedback);
            string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);

            OnWriteMessage(result);

            Thread.Sleep(3000);

            string error = 0.ToString("X04");
            int executeTime = _rd.Next(10000, 999999);
            string exeTime = executeTime.ToString("D6");
            string posData = GetPosData();
            string valueData = GetValueData();

            feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},{7},", unit, seq, sts, error, cmd, exeTime, posData, valueData);
            sum = CheckSum(feedback);
            result = String.Format("{0}{1}{2}", "!", feedback, sum);

            OnWriteMessage(result);
        }

        private void HandleMREL(string obj)
        {
            Response("MREL", obj);
            EndOfExecution(obj);
        }

        //$,<UNo>(,<SeqNo>),INIT,<ErrClr>,<SrvOn>,<Home>(,<Sum>)<CR>
        //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,INIT,<ErrClr>,<SrvOn>,<Axis>(,<Sum>)<CR>
        //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,INIT,<ExeTime>,<PosData1>…,<PosDataN>(,<Sum>)<CR>
        public virtual void HandleINIT(string msg)
        {
            string[] arrayMsg = msg.Split(',');
            string unit = arrayMsg[1];
            string seq = arrayMsg[2];
            string cmd = arrayMsg[3];
            string sts = 0.ToString("X02");
            string Ackcd = 0.ToString("X04");
            string errorClr = arrayMsg[4];
            string srvOn = arrayMsg[5];
            string axis = arrayMsg[6];

            string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},{7},", unit, seq, sts, Ackcd, cmd, errorClr, srvOn, axis);
            string sum = CheckSum(feedback);
            string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);

            OnWriteMessage(result);


            Thread.Sleep(3000);

            int executeTime = _rd.Next(10000, 999999);
            string exeTime = executeTime.ToString("D6");
            string posData = GetPosData();
            string error = 0.ToString("X04");

            feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, cmd, exeTime, posData);
            sum = CheckSum(feedback);
            result = String.Format("{0}{1}{2}", "!", feedback, sum);

            OnWriteMessage(result);

        }

        //$,0     ,1       ,2    ,3      ,4        ,5          ,6        ,7     <CR>  
        //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,<Command>,<Parameter>, <Value>(,<Sum>)<CR>
        //
        public void Response(string cmd, string msg)
        {
            string[] arrayMsg = msg.Split(',');
            string unit = arrayMsg[1];
            string seq = arrayMsg[2];
            string sts = 0.ToString("X02");
            string error = 0.ToString("X04");
            string Ackcd = 0.ToString("X04");
            int status1 = Convert.ToInt32("0011", 2);
            string status = string.Format("{0}000", status1.ToString("X1"));
            string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, Ackcd, cmd, error, status);
            string sum = CheckSum(feedback);
            string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);

            OnWriteMessage(result);
        }

        public string CheckSum(string feedback)
        {
            int sum = 0;
            foreach (var item in feedback)
            {
                sum += (int)item;
            }

            sum = sum % 256;
            return sum.ToString("X02");
        }

        public void EndOfExecution(string msg)
        {
            string[] arrayMsg = msg.Split(',');
            string unit = arrayMsg[1];
            string seq = arrayMsg[2];
            string sts = 0.ToString("X02");
            string error = 0.ToString("X04");
            string type = arrayMsg[3];
            int executeTime = _rd.Next(10000, 999999);
            string exeTime = executeTime.ToString("D6");
            string posData = 0.ToString("D8");
            int status1 = Convert.ToInt32("0011", 2);
            string status = string.Format("{0}000", status1.ToString("X1"));
            string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, type, exeTime, posData);
            string sum = CheckSum(feedback);
            string result = String.Format("{0}{1}{2}", "!", feedback, sum);

            OnWriteMessage(result);
        }
 

        public string GetPosData()
        {
            return string.Format("{0},{1},{2},{3},{4}", _rd.Next().ToString("D8"), _rd.Next().ToString("D8"), _rd.Next().ToString("D8"), _rd.Next().ToString("D8"), _rd.Next().ToString("D8"));
        }

        public string GetValueData()
        {
            return string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9}",
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"),
                _rd.Next(0, 0).ToString("D8"));
        }

        public string GetMapData(bool random, bool full)
        {
            string result="";
            for (int i = 0; i < 25; i++)
            {
                if (random)
                {
                    result += string.Format("{0:00}:{1}", i + 1, _rd.Next(100) > 50 ? "OK" : "--");
                }else if (full)
                {
                    result += string.Format("{0:00}:{1}", i + 1,  "OK"  );
                }
                else
                {
                    result += string.Format("{0:00}:{1}", i + 1, "--");

                }
                if (i < 24)
                {
                    result += ",";
                }
            }

            return result;
        }

        public string GetMapData(bool pre, bool post, bool mid)
        {
            string result = "";
            for (int i = 0; i < 25; i++)
            {
                if (i < 8)
                {
                    result += string.Format("{0:00}:{1}", i + 1, pre ? "OK" : "--");
                }
                else if (i < 16)
                {
                    result += string.Format("{0:00}:{1}", i + 1, mid ? "OK" : "--");
                }
                else
                {
                    result += string.Format("{0:00}:{1}", i + 1, post ? "OK" : "--");
                }

                if (i < 24)
                {
                    result += ",";
                }
            }

            return result;
        }
    }
}