PunkHPX8/Framework/SimulatorCore/Robots/BrooksMag7RobotSimulator.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Timers;
using Aitex.Core.Util;
using MECF.Framework.Simulator.Core.Driver;
using MECF.Framework.Simulator.Core.SubstrateTrackings;

namespace MECF.Framework.Simulator.Core.Robots
{
 
    public class BrooksMag7RobotSimulator : RobotSimulator
    {
        protected Random _rd = new Random();
        public bool Failed { get; set; }
        public string ResultValue { get; set; }

        public override Dictionary<string, double> MoveTimes
        {
            get { return moveTimes; }
            set { moveTimes = value; }
        }

        public override ReadOnlyCollection<string> Arms
        {
            get { return arms; }
        }

        //private static string source = "BrooksMag7";
        private static string msgDone = "_RDY";
        private static string msgError = "_ERR";
        private readonly string armAPan1 = "VTM.ArmA.Left";
        private readonly string armAPan2 = "VTM.ArmA.Right";
        private readonly string armBPan1 = "VTM.ArmB.Left";
        private readonly string armBPan2 = "VTM.ArmB.Right";
        private System.Timers.Timer timer;
         private string currentStation;
        private string newLocation;
        private string currentArm;
        private string newArm = "";
        private string lastMsg;
        private string armTargetMaterialMap;        //jms changed from "picking" for CR#7576.
        private Dictionary<string, double> moveTimes;
        private ReadOnlyCollection<string> arms;
 
        //private bool lidClosed;

 

        public BrooksMag7RobotSimulator( ):base(1102, 0, "\r", ' ')
        {
 
            List<string> armsList = new List<string>();
            armsList.Add(armAPan1);
            armsList.Add(armAPan2);
            armsList.Add(armBPan1);
            armsList.Add(armBPan2);
            arms = new ReadOnlyCollection<string>(armsList);

            //WaferTrack.Instance.UpdateMaterialMap(armAPan1, WaferTrackStateEnum.Unoccupied);
            //WaferTrack.Instance.UpdateMaterialMap(armAPan2, WaferTrackStateEnum.Unoccupied);
            //WaferTrack.Instance.UpdateMaterialMap(armBPan1, WaferTrackStateEnum.Unoccupied);
            //WaferTrack.Instance.UpdateMaterialMap(armBPan2, WaferTrackStateEnum.Unoccupied);

 
            // create the message handling dictionary
            AddCommandHandler("HOME",  HandleHome);
            AddCommandHandler("PICK",  HandlePick);
            AddCommandHandler("PLACE",  HandlePlace);
            AddCommandHandler("GOTO",  HandleGoto);
            AddCommandHandler("SWAP",  HandleExchange);
            AddCommandHandler("RQ",  HandleRequest);
            AddCommandHandler("Unknown",  HandleUnknown);

            timer = new System.Timers.Timer();
            timer.Enabled = false;
            timer.AutoReset = false;
            timer.Elapsed += new ElapsedEventHandler(timer_Elapsed);

            // Default move times based on Brooks spreadsheet
            moveTimes = new Dictionary<string, double>();
            moveTimes["PickPlace"] = 2.8;
            moveTimes["Move90Degrees"] = 1.69;
            moveTimes["Move180Degrees"] = 2.11;
            moveTimes["HeightAdjust"] = 0.9;
            moveTimes["ExtendRetract"] = 1.3;
            moveTimes["WaferFactor"] = 1.0;
            moveTimes["SwapAtPM"] = 7.3;

            // Original default times based on Brooks log files
            //moveTimes["PickPlace"] = 3.2;
            //moveTimes["Move90Degrees"] = 1.5;
            //moveTimes["Move180Degrees"] = 1.9;
            //moveTimes["HeightAdjust"] = 0.9;
            //moveTimes["ExtendRetract"] = 1.2;
            //moveTimes["WaferFactor"] = 1.15;

            currentArm = "A";

            currentStation = "Unknown";

             //lidClosed = true;

 
            

        }

        public void HomeAll()
        {
            string msg = "HOME ALL";
            HandleHome(msg);
        }

        public void Pick()
        {
            string msg = "PICK 1 SLOT 1 ARM A";
            HandlePick(msg);
        }

        public void Place()
        {
            string msg = "PLACE STN 2 ARM A";
            HandlePlace(msg);
        }

        internal void HandleHome(string msg)
        {
 
                if (ErrorMessage == "Home Failed")
                {
                    HandleError(ErrorMessage);
                    return;
                }

                string[] cmdComponents = msg.Split(_msgDelimiter);
                if (cmdComponents.Length != 2)
                {
                    // return an error
                    HandleError("Invalid homing command (arguments)");
                    return;
                }
                HandleMove(RobotStateEnum.Homing, cmdComponents);
 
        }

 
        internal void HandlePick(string msg)
        {
 
                msg = msg.Trim();
                if (ErrorMessage == "Pick Failed")
                {
                    HandleError(ErrorMessage);
                    return;
                }

                string[] cmdComponents = msg.Split(_msgDelimiter);
                if (cmdComponents.Length != 6 && cmdComponents.Length != 8 && cmdComponents.Length != 10)
                {
                    //Have to check for 6 parameters (double pick) or 8 parameters (single pick)
                    // return an error
                     
                    HandleError("Invalid pick command (arguments)");
                    return;
                }

                RobotStateEnum rs = RobotStateEnum.Picking;
                if (cmdComponents.Length > 6 && cmdComponents[6] == "ENRT")
                    rs = RobotStateEnum.Extending;
                else if (cmdComponents.Length > 6 && cmdComponents[6] == "STRT")
                    rs = RobotStateEnum.Retracting;

                lastMsg = msg;
                armTargetMaterialMap = "pick";      //jms changed to string for CR#7576.
                HandleMove(rs, cmdComponents);
 
        }
 
        internal void HandlePlace(string msg)
        {
 
                msg = msg.Trim();
                if (ErrorMessage == "Place Failed")
                {
                    HandleError(ErrorMessage);
                    return;
                }

                string[] cmdComponents = msg.Split(_msgDelimiter);
                if (cmdComponents.Length != 6 && cmdComponents.Length != 8 && cmdComponents.Length != 10)
                {
                    //Have to check for 6 parameters (double pick) or 8 parameters (single pick)
                    // return an error
                   // Log.WriteIfEnabled(LogCategory.Error, source, "Unrecognized place command: " + msg);
                    HandleError("Invalid place command (arguments)");
                    return;
                }

                RobotStateEnum rs = RobotStateEnum.Placing;
                if (cmdComponents.Length > 6 && cmdComponents[6] == "ENRT")
                    rs = RobotStateEnum.Extending;
                else if (cmdComponents.Length > 6 && cmdComponents[6] == "STRT")
                    rs = RobotStateEnum.Retracting;

                lastMsg = msg;
                armTargetMaterialMap = "place";      //jms changed to string for CR#7576.
                HandleMove(rs, cmdComponents);
  
        }
 
        internal void HandleExchange(string msg)
        {
            //jms enabled for CR#7576.
 
                if (ErrorMessage == "Swap Failed" || ErrorMessage == "Place Failed" || ErrorMessage == "Pick Failed")
                {
                    HandleError(ErrorMessage);
                    return;
                }

                string[] cmdComponents = msg.Split(_msgDelimiter);
                if (cmdComponents.Length != 4)
                {
                    //Log.WriteIfEnabled(LogCategory.Error, source, "Unrecognized swap command: " + msg);
                    HandleError("Invalid swap command (arguments)");
                    return;
                }
                lastMsg = msg;
                armTargetMaterialMap = "swap";
                //Log.WriteIfEnabled(LogCategory.Error, source, "HandleExchange msg: " + msg);
                HandleMove(RobotStateEnum.Exchanging, cmdComponents);
 
        }

 
        internal void HandleRequest(string msg)
        {
            string[] components = msg.Split(_msgDelimiter);
            string reply = components[1];
            if (components[1] == "WAFER" && components[2] == "PRESENT")
                reply += " " + components[2] + GetArmStates();
            else if (components[1] == "ERRMSG")
                reply = LookupError(components[2]);

            //Log.WriteIfEnabled(LogCategory.Debug, source, "Writing message " + reply);
            OnWriteMessage(reply);
            //Log.WriteIfEnabled(LogCategory.Debug, source, "Writing message _RDY");
            OnWriteMessage(msgDone);
        }

 
        internal void HandleUnknown(string msg)
        {
            //Log.WriteIfEnabled(LogCategory.Debug, source, "Command " + msg + "complete. Writing message _RDY");
            OnWriteMessage(msgDone);
        }

 
        internal void HandleGoto(string msg)
        {
 
                string[] cmdComponents = msg.Split(_msgDelimiter);
                if (cmdComponents.Length != 11)
                {
                    // return an error
                    HandleError("Invalid move command (arguments)");
                    return;
                }
                HandleMove(RobotStateEnum.Approaching, cmdComponents);
  
        }

        /// <summary>
        /// Simulates moves for move messages
        /// </summary>
        /// <param name="action">Action to perform (pick, home, place, goto)</param>
        /// <param name="cmdComponents">Components</param>
        /// <returns>True if successful, otherwise false.</returns>
        private bool HandleMove(RobotStateEnum action, string[] cmdComponents)
        {
 
                // if the robot is moving, send an error message
                if (robotStateArgs.State != RobotStateEnum.Idle &&
                    (action == RobotStateEnum.Homing && robotStateArgs.State != RobotStateEnum.Errored))  // allow homes when in error, but not other moves
                {
                    // return an error
                    HandleError("Already moving");
                    return false;
                }

                newLocation = "Unknown";
                switch (action)
                {
                    case RobotStateEnum.Picking:
                    case RobotStateEnum.Placing:
                    case RobotStateEnum.Approaching:
                    case RobotStateEnum.Extending:
                    case RobotStateEnum.Retracting:
                    case RobotStateEnum.Exchanging:          //jms added exchanging for CR#7576.
                    //Log.WriteIfEnabled( LogCategory.Information, source, "Robot {0}: {1} {2} {3} {4}", action, cmdComponents[2], cmdComponents[3], cmdComponents[4], cmdComponents[5] );
                        //armIndex = Array.IndexOf(cmdComponents, "ARM"); 
                        //armIndex++;
                        //newArm = cmdComponents[armIndex];   //指定动作机械臂
                        //int index = action == RobotStateEnum.Approaching ? 2 : 1;
                        //newLocation = GetLocation(cmdComponents[index]);

                        //if (RobotStateEnum.Approaching == action)
                        //{
                        //    //Log.WriteIfEnabled(LogCategory.Debug, source, string.Format("Moving from {0} to {1}", currentStation, newLocation));
                        //}

                        //int slotIndex = Array.IndexOf(cmdComponents, "SLOT" ) + 1;
                        //Log.WriteIfEnabled(LogCategory.Information, source, string.Format("VTM robot {0} {1} slot {2} arm {3}",action, newLocation, cmdComponents[slotIndex], newArm));
                        //bool state;
                        //if (cmdComponents[armIndex] == "A")
                        //    state = (WaferTrack.Instance.IsOccupied(armAPan1) || WaferTrack.Instance.IsOccupied(armAPan2));
                        //else
                        //    state = (WaferTrack.Instance.IsOccupied(armBPan1) || WaferTrack.Instance.IsOccupied(armBPan2));

                        //if (action == RobotStateEnum.Picking && state)
                        //{
                        //    HandleError("Already holding wafer");
                        //    return false;
                        //}
                        //else if (action == RobotStateEnum.Placing && !state)
                        //{
                        //    HandleError("Not holding wafer");
                        //    return false;
                        //}
                        break;

                    default:
                        //Log.WriteIfEnabled(LogCategory.Information, source, string.Format("VTM robot {0}", action));
                        break;
                }

                //jms added swap check for CR#7576.
                if (ErrorMessage != "Pick Failed" && ErrorMessage != "Place Failed" && ErrorMessage != "Home Failed" &&
                    ErrorMessage != "Swap Failed" && !string.IsNullOrEmpty(ErrorMessage))
                {
                    HandleError(ErrorMessage);
                    return false;
                }

                //SetRobotState(action);                 // set state to what the robot is now doing

                double delay = GetMoveTime(action);

                //Log.WriteIfEnabled(LogCategory.Debug, source, "Move delay in seconds: " + delay.ToString());
                timer.Interval = delay * 1000;
                timer.Enabled = true;
 

            return true;
  
        }

        private static string GetLocation(string specifier)
        {
            switch (specifier)
            {
                case "1":
                    return "LL1";

                case "2":
                    return "LL2";

                case "3":
                    return "PM1";

                case "4":
                    return "PM2";

                case "5":
                    return "PM3";
            }

            return "Unknown";
        }

        private double GetMoveTime(RobotStateEnum action)
        {
            //if (WaferTrack.Instance.RealisticMode == false)
            //    return 0.0;

            double rotationTime = 0;
            double zMoveTime = 0;
            if (newLocation != currentStation)
            {
                if (currentStation == "LL1" || currentStation == "LL2")
                    rotationTime = newLocation == "PM2" ? moveTimes["Move180Degrees"] : moveTimes["Move90Degrees"];
                else
                    rotationTime = currentStation == "PM2" ? moveTimes["Move180Degrees"] : moveTimes["Move90Degrees"];
                double factor = 1.0;
                if (WaferTrack.Instance.IsOccupied(armAPan1) || WaferTrack.Instance.IsOccupied(armAPan2) ||
                    WaferTrack.Instance.IsOccupied(armBPan1) || WaferTrack.Instance.IsOccupied(armBPan2))
                {
                    factor = moveTimes["WaferFactor"];
                }

                rotationTime *= factor;
            }
            else if (newArm != currentArm)
                zMoveTime = moveTimes["HeightAdjust"];

            switch (action)
            {
                case RobotStateEnum.Approaching:
                    if (newLocation == currentStation)
                        return moveTimes["HeightAdjust"];

                    return rotationTime;

                case RobotStateEnum.Extending:
                case RobotStateEnum.Retracting:
                    return moveTimes["ExtendRetract"] + rotationTime + zMoveTime;

                case RobotStateEnum.Picking:
                    return moveTimes["PickPlace"] + zMoveTime;

                case RobotStateEnum.Placing:
                    if (newLocation.StartsWith("PM"))
                        return moveTimes["PickPlace"] + zMoveTime;
                    else
                        return moveTimes["ExtendRetract"] + rotationTime + zMoveTime;

                case RobotStateEnum.Exchanging:
                    return moveTimes["SwapAtPM"];

                default:
                    return moveTimes["Move90Degrees"];
            }
        }


        /// <summary>
        /// Method for returning error messages
        /// </summary>
        /// <param name="msg">Error message to return</param>
        private void HandleError(string msg)
        {
            string errorCode = string.Format("0x{0}", lastError.ToString("x8"));
            lastError++;
            errorLookup[errorCode] = msg;
            //Log.WriteIfEnabled(LogCategory.Debug, source, "Writing error message " + msg);
            OnWriteMessage(msgError + " " + errorCode);
            //Log.WriteIfEnabled(LogCategory.Debug, source, "Command complete. Writing message _RDY");
            OnWriteMessage(msgDone);
        }

        /// <summary>
        /// Set the state of the arm based on the message
        /// </summary>
        /// <param name="msg">Message that causes the state to change</param>
        /// <param name="state">New state of the arm</param>
        private void SetArmState(string msg, string pickPlaceSwap)
        {
                //string[] components = msg.Split(_msgDelimiter);
                ////jms added swap check (length == 4) for CR#7576.
                //if (components.Length != 6 && components.Length != 8 && components.Length != 10 && components.Length != 4)
                //    return;

                //int slotIndex = Array.IndexOf(components, "SLOT" ) + 1;
                //bool isLL = newLocation.StartsWith("LL");
                //string target1 = string.Format("{0}.{1}{2}", newLocation, isLL ? "Ch2" : "Ch1", isLL ? ".Slot" + components[slotIndex] : string.Empty);
                //string target2 = string.Format("{0}.{1}{2}", newLocation, isLL ? "Ch1" : "Ch2", isLL ? ".Slot" + components[slotIndex] : string.Empty);
                //string arm = "VTM.Arm" + components[armIndex];

                ////jms changed pickPlaceSwap to string for CR#7576.
                //if (pickPlaceSwap == "pick")
                //{
                //    WaferTrack.Instance.UpdateMaterialMap(arm + ".Left", WaferTrack.Instance.GetLocationState(target1));
                //    WaferTrack.Instance.UpdateMaterialMap(arm + ".Right", WaferTrack.Instance.GetLocationState(target2));
                //    WaferTrack.Instance.UpdateMaterialMap(target1, WaferTrackStateEnum.Unoccupied);
                //    WaferTrack.Instance.UpdateMaterialMap(target2, WaferTrackStateEnum.Unoccupied);
                //}
                //else if (pickPlaceSwap == "place")
                //{
                //    WaferTrack.Instance.UpdateMaterialMap(target1, WaferTrack.Instance.GetLocationState(arm + ".Left"));
                //    WaferTrack.Instance.UpdateMaterialMap(target2, WaferTrack.Instance.GetLocationState(arm + ".Right"));
                //    WaferTrack.Instance.UpdateMaterialMap(arm + ".Left", WaferTrackStateEnum.Unoccupied);
                //    WaferTrack.Instance.UpdateMaterialMap(arm + ".Right", WaferTrackStateEnum.Unoccupied);
                //}
                ////jms added swap check for CR#7576.
                //else if (pickPlaceSwap == "swap")
                //{
                //    if (components[armIndex] == "A")
                //    {
                //        WaferTrack.Instance.UpdateWaferTrackState(armAPan1, WaferTrack.Instance.GetLocationState(target1));
                //        WaferTrack.Instance.UpdateMaterialMap(armAPan2, WaferTrack.Instance.GetLocationState(target2));
                //        WaferTrack.Instance.UpdateMaterialMap(target1, WaferTrack.Instance.GetLocationState(armBPan1));
                //        WaferTrack.Instance.UpdateMaterialMap(target2, WaferTrack.Instance.GetLocationState(armBPan2));
                //        WaferTrack.Instance.UpdateMaterialMap(armBPan1, WaferTrackStateEnum.Unoccupied);
                //        WaferTrack.Instance.UpdateMaterialMap(armBPan2, WaferTrackStateEnum.Unoccupied);
                //    }
                //    else
                //    {
                //        WaferTrack.Instance.UpdateMaterialMap(armBPan1, WaferTrack.Instance.GetLocationState(target1));
                //        WaferTrack.Instance.UpdateMaterialMap(armBPan2, WaferTrack.Instance.GetLocationState(target2));
                //        WaferTrack.Instance.UpdateMaterialMap(target1, WaferTrack.Instance.GetLocationState(armAPan1));
                //        WaferTrack.Instance.UpdateMaterialMap(target2, WaferTrack.Instance.GetLocationState(armAPan2));
                //        WaferTrack.Instance.UpdateMaterialMap(armAPan1, WaferTrackStateEnum.Unoccupied);
                //        WaferTrack.Instance.UpdateMaterialMap(armAPan2, WaferTrackStateEnum.Unoccupied);
                //    }
                //}
        }

        /// <summary>
        /// Get the arm state string returned in WAFER PRESENT request message
        /// </summary>
        /// <returns>Arm state string</returns>
        private string GetArmStates()
        {
            return " N N";
 
                //string states = " A LEFT " + (WaferTrack.Instance.IsOccupied(armAPan1) ? "Y" : "N");
                //states += " RIGHT " + (WaferTrack.Instance.IsOccupied(armAPan2) ? "Y" : "N");
                //states += " B LEFT " + (WaferTrack.Instance.IsOccupied(armBPan1) ? "Y" : "N");
                //states += " RIGHT " + (WaferTrack.Instance.IsOccupied(armBPan2) ? "Y" : "N");

                //return states;
 
        }

        /// <summary>
        /// Timer used for simulating realistic mode
        /// </summary>
        /// <param name="sender">Who sent it</param>
        /// <param name="e">Arguments</param>
        private void timer_Elapsed(object sender, ElapsedEventArgs e)
        {
 
                //Log.WriteIfEnabled(LogCategory.Debug, source, string.Format("Move complete to station {0}. Writing message _RDY", newLocation));
                currentStation = newLocation;
                currentArm = newArm;
                //jms added exchanging check for CR#7576.
                if (robotStateArgs.State == RobotStateEnum.Picking || robotStateArgs.State == RobotStateEnum.Placing ||
                          robotStateArgs.State == RobotStateEnum.Extending || robotStateArgs.State == RobotStateEnum.Exchanging)
                {
                    SetArmState(lastMsg, armTargetMaterialMap);
                    lastMsg = "";
                }
                SetRobotState(RobotStateEnum.Idle);
                timer.Enabled = false;
                OnWriteMessage(msgDone);
 
        }

 

    }
}