CyberX8/CyberX8_RT/Modules/Transporter/TransporterPickUpFromRoutine.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using MECF.Framework.Common.Beckhoff.Station;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Layout;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.Utilities;
using MECF.Framework.Common.WaferHolder;
using CyberX8_Core;
using CyberX8_RT.Devices.AXIS;
using CyberX8_RT.Devices.AXIS.CANOpen;
using CyberX8_RT.Devices.Facilities;
using CyberX8_RT.Devices.Loader;
using CyberX8_RT.Devices.TransPorter;
using CyberX8_RT.Modules.Loader;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using CyberX8_RT.Devices.Rinse;
using CyberX8_RT.Devices.Metal;
using Aitex.Core.Common;

namespace CyberX8_RT.Modules.Transporter
{
   
    public class TransporterPickUpFromRoutine : RoutineBase, IRoutine
    {
        private enum PickUpStep
        {
            CheckPreCondition,
            DropBlockLockoff,
            ElevatorGotoUp,
            ElevatorGotoUpWait,
            SafeMoveTo,
            CheckMoveToStatus,
            GantryPosition,
            GantryPoisitionWait,           
            ElevatorPositionToCell,
            ElevatorPositionToCellWait,
            Delay,
            DropBlockLockOn, 
            CalculateLiftupSpeed,
            PickupDelay,
            ElevatorPositionToUp,
            ElevatorPositionToUpWait,
            UpdateWaferShuttle,
            CheckWSPresent,
            //ReadBarcodeConfirm,
            End
        }

        #region 内部变量
        private string _cellName;
        private JetAxisBase _gantryAxis;
        private JetAxisBase _elevatorAxis;
        private LoaderEntity _loaderEntity;
        private JetAxisBase _loaderRotationAxis;
        private SystemFacilities _facilities;
        private TransporterConflictRoutine _conflictRoutine;
        private TransporterCommon _transporterCommon;
        private LoaderCommonDevice _loaderCommonDevice;
        ProcessLayoutCellItem _cellItem;
        private int _pickupTime;
        private int _pickupDelayTime;
        private int _velocity;
        private int _acceleration;
        private bool _bypassWaferHolderPresent;
        private int _pickMaxRetries = 2;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public TransporterPickUpFromRoutine(string module) : base(module)
        {
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(PickUpStep.CheckPreCondition, CheckStartPreConfition, _delay_1ms)
                //1. 确认DropBlockLock是否off
                .Run(PickUpStep.DropBlockLockoff, DropBlockLockoff, _delay_1ms)
                //2. Elevator go to up
                .Run(PickUpStep.ElevatorGotoUp, ElevatorGotoUP, _delay_1ms)
                .WaitWithStopCondition(PickUpStep.ElevatorGotoUpWait, CheckElevatorPositionEndStatus, CheckElevatorPositionEndStatus)
                //3. other Transporter Safe Move 
                .Run(PickUpStep.SafeMoveTo, SafeMoveTo, _delay_1ms)
                .WaitWithStopCondition(PickUpStep.CheckMoveToStatus, () => CommonFunction.CheckRoutineEndState(_conflictRoutine),
                CheckSafeMoveToStopStatus)
                //4. Gantry 移动到目标位置
                .Run(PickUpStep.GantryPosition, GantryPositionToCell, _delay_1ms)
                .WaitWithStopCondition(PickUpStep.GantryPoisitionWait, CheckGantryPositionStatus, CheckGantryPositionRunStop)                
                //5. Elevator 移动至对应cell位
                .Run(PickUpStep.ElevatorPositionToCell, ElevatorPositionToCell, _delay_1ms)
                .WaitWithStopCondition(PickUpStep.ElevatorPositionToCellWait, CheckElevatorPositionEndStatus, CheckElevatorPositionStopStatus)
                .Delay(PickUpStep.Delay,500)
                //6. Drop Block Lock on，抓取WS
                .Run(PickUpStep.DropBlockLockOn, DropBlockLockon, _delay_1ms)              
                //7. 确认Elevator lift speed
                .Run(PickUpStep.CalculateLiftupSpeed,CalculateLiftupSpeed,_delay_1ms)
                //8. Pickup delay
                .Delay(PickUpStep.PickupDelay,_pickupDelayTime * 1000)
                //9. Elevator goto Up
                 .Run(PickUpStep.ElevatorPositionToUp, ElevatorGotoUP, 100)
                .WaitWithStopCondition(PickUpStep.ElevatorPositionToUpWait, CheckElevatorPositionEndStatus, CheckElevatorPositionStopStatus)
                //10. Material Tracking Update
                .Run(PickUpStep.UpdateWaferShuttle, WaferShuttleTransfer,_delay_1ms)
                //11. 确认Transporter的Wafer shuttle hold present信号为on
                .Wait(PickUpStep.CheckWSPresent, CheckWSPresent, _delay_2s)
                //13. Read Barcode 确认与Material Tracking的转移是否一致
                //.Run(PickUpStep.ReadBarcodeConfirm,ReadBarcode,_delay_1ms)
                .End(PickUpStep.End,NullFun,100);
            return Runner.Status;
        }
        /// <summary>
        /// 关闭DropBlockLock
        /// </summary>
        /// <returns></returns>
        private bool DropBlockLockoff()
        {
            if (_transporterCommon.TransporterData.Lock)
            {
                return _transporterCommon.UnlockOperation("", null);
            }       
            return true;
        }
        /// <summary>
        /// 打开DropBlockLock
        /// </summary>
        /// <returns></returns>
        private bool DropBlockLockon()
        {
            if (!_transporterCommon.TransporterData.Lock)
            {
                return _transporterCommon.LockOperation("", null);
            }
            return true;
        }
        /// <summary>
        /// 安全避障移动
        /// </summary>
        /// <returns></returns>
        private bool SafeMoveTo()
        {
            _cellItem = ProcessLayoutManager.Instance.GetProcessLayoutCellItemByModuleName(_cellName);
            string stationName = _cellName;
            if (_cellItem != null)
            {
                if (_cellName.ToLower() != "loader" && _cellName.ToLower() != "park")
                {
                    stationName = $"Cell{_cellItem.CellId}";
                }
            }
            else
            {
                NotifyError(eEvent.ERR_TRANSPORTER,  $"{_cellName} not in layout", 0);
                return false;
            }
            var result = _gantryAxis.GetPositionByStation(stationName);
            if(result.success)
            {
                bool isPositive = false;
                if(_gantryAxis.MotionData.MotorPosition<result.position)
                {
                    isPositive = true;
                }
                return _conflictRoutine.Start(result.position, isPositive) == RState.Running;
            }
            else
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{_cellName} not in station list",0);
                return false;
            }
        }
        /// <summary>
        /// 检验安全避让异常结束状态
        /// </summary>
        /// <returns></returns>
        private bool CheckSafeMoveToStopStatus()
        {
            bool result = CommonFunction.CheckRoutineStopState(_conflictRoutine);
            if (result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "Safe Move failed", 0);
            }
            return result;
        }
        /// <summary>
        /// Gantry Position To cell
        /// </summary>
        /// <returns></returns>
        private bool GantryPositionToCell()
        {
            _cellItem= ProcessLayoutManager.Instance.GetProcessLayoutCellItemByModuleName(_cellName);
            bool result = false;
            if (_cellItem != null)
            {
                if (_cellName.ToLower() != "loader"&&_cellName.ToLower()!="park")
                {
                    result= _gantryAxis.PositionStation($"Cell{_cellItem.CellId}", false);
                }
                else
                {
                    result= _gantryAxis.PositionStation(_cellName,false);
                }
                if (!result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "gantry axis motion failed", 0);
                }
                return result;
            }
            else
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{_cellName} not in layout", 0);
                return false;
            }
        }
        /// <summary>
        /// Elevator Position to cell
        /// </summary>
        /// <returns></returns>
        private bool ElevatorPositionToCell()
        {
            bool result = false;
            if( _cellItem != null )
            {
                if (_cellName.ToLower()!="loader")
                {
                    result= _elevatorAxis.PositionStation($"Cell{_cellItem.CellId}", false);
                }
                else
                {
                    result= _elevatorAxis.PositionStation(_cellName, false);
                }
                if (!result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "elevator axis motion failed", 0);
                }
                return result;
            }
            else
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{_cellName} not in layout",0);
                return false;
            }
        }
        /// <summary>
        /// 检验Vertical移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckElevatorPositionEndStatus()
        {
            return _elevatorAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Vertical是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckElevatorPositionStopStatus()
        {
            bool result = _elevatorAxis.Status == RState.Failed || _elevatorAxis.Status == RState.Timeout ;
            if (result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "elevator axis motion failed",0);
            }
            return result;
        }
        /// <summary>
        /// 检验Gantry移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckGantryPositionStatus()
        {
            return _gantryAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Gantry是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckGantryPositionRunStop()
        {
            bool result= _gantryAxis.Status == RState.Failed||_gantryAxis.Status==RState.Timeout;
            if(result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "Gantry Motion failed", 0);
            }
            return result;
        }        
        /// <summary>
        /// 计算提升速度
        /// </summary>
        /// <returns></returns>
        private bool CalculateLiftupSpeed()
        {
            if(_pickupTime==0)
            {
                return true;
            }  
            BeckhoffStationAxis stationAxis = BeckhoffStationLocationManager.Instance.GetStationAxis(Module, "Elevator", 0);
            Station upStation = stationAxis.Stations.Find(O => O.Name.EndsWith("UP"));
            Station cellStation = null;
            if (_cellName == "Loader")
            {
                cellStation = stationAxis.Stations.Find(O => O.Name.EndsWith("Loader"));
            }
            else
            {
                cellStation = stationAxis.Stations.Find(O => O.Name.EndsWith($"Cell{_cellItem.CellId}"));
            }
            if(upStation==null)
            {
                NotifyError(eEvent.ERR_TRANSPORTER,  "UP is not in Station List",0);
                return false;
            }
            if(cellStation==null)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"Cell{_cellItem.CellId} is not in Station List",0);
                return false;
            }
            if(!double.TryParse(cellStation.Position,out double cellStationPosition))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"Cell{_cellItem.CellId} Station Position is invalid",0);
                return false;
            }
            if (!double.TryParse(upStation.Position, out double upStationPosition))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"Cell{_cellItem.CellId} Station Position is invalid", 0);
                return false;
            }
            int distance =(int)Math.Round(Math.Abs(cellStationPosition - upStationPosition));
            double tmpVelocity = (double) distance/ _pickupTime;
            _velocity = _elevatorAxis.CalculateMultiplySpeedRatio(_elevatorAxis.CalculateValueMultiplyScale(tmpVelocity));
            double tmpAccelaration = (double)(2 * distance) / Math.Pow(_pickupTime, 2);
            _acceleration = _elevatorAxis.CalculateMultiplyAccelerationRatio(_elevatorAxis.CalculateValueMultiplyScale(tmpAccelaration));
            LOG.WriteBackgroundLog(eEvent.INFO_TRANSPORTER, Module, "adjust profile speed");
            return true;
        }
        /// <summary>
        /// Elevator运动至UP
        /// </summary>
        /// <returns></returns>
        private bool ElevatorGotoUP()
        {
            bool result= _elevatorAxis.PositionStation("UP", false, _velocity, _acceleration, _acceleration);
            if(!result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "elevator goto up failed",0);
            }
            return result;
        }
        /// <summary>
        /// 更新WaferHolder移动信息
        /// </summary>
        /// <returns></returns>
        private bool WaferShuttleTransfer()
        {
            bool isMetal = false;
            DateTime lastMetalRecipeTime = DateTime.Now;
            if(Enum.TryParse(_cellName,out ModuleName moduleName)&&ModuleHelper.IsMetal(moduleName))
            {
                isMetal = true;
                WaferHolderInfo info = WaferHolderManager.Instance.GetWaferHolder(_cellName);
                if (info != null)
                {
                    lastMetalRecipeTime = info.LastMetalRecipeCompleteTime;
                }
            }
            bool result= WaferHolderManager.Instance.TransferWaferHolder(Module,_cellName, Module);
            if (!result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "tranfer waferHolder message failed", 0);
            }
            else
            {
                if (!Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    return true;
                }
                WaferHolderInfo waferHolderInfo = WaferHolderManager.Instance.GetWaferHolder(Module);
                if (waferHolderInfo == null)
                {
                    return true;
                }
                string strTime = DateTime.Now.ToString("yyyy/MM/dd HH:mm:ss");
                waferHolderInfo.SchedulerModules.Add($"{strTime} {_cellName} => {Module}");
                if (isMetal)
                {
                    int waferShuttleSoakMaxTime = SC.GetValue<int>("Metal.WaferShuttleSoakMaxTime");
                    double soakTime = Math.Round(DateTime.Now.Subtract(lastMetalRecipeTime).TotalMinutes,2);
                    if (soakTime > waferShuttleSoakMaxTime)
                    {
                        LOG.WriteLog(eEvent.WARN_METAL, _cellName, $"time of {waferHolderInfo.Id} soak in metal is {soakTime} minute over {waferShuttleSoakMaxTime}");
                    }
                    else
                    {
                        LOG.WriteLog(eEvent.INFO_METAL, _cellName, $"time of {waferHolderInfo.Id} soak in metal is {soakTime} minute");
                    }
                }
            }

            return result;
        }
        /// <summary>
        /// 检验WHPresent与Lock状态
        /// </summary>
        /// <returns></returns>
        private bool CheckWSPresent()
        {
            bool locked = _transporterCommon.TransporterData.Lock;
            bool whPresent= _transporterCommon.TransporterData.WSHoldPresent;
            bool result= locked && (whPresent||_bypassWaferHolderPresent);
            if (!result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "check wafer Shuttle present and locked failed", 0);
            }
            return result;
        }
        /// <summary>
        /// 读取条码
        /// </summary>
        /// <returns></returns>
        private bool ReadBarcode()
        {
            string str=_transporterCommon.ReaderBarcode();
            str = str.Trim();
            if(!string.IsNullOrEmpty(str))
            {
                TransporterEntity transporterEntity = Singleton<RouteManager>.Instance.GetModule<TransporterEntity>(Module.ToString());
                if (transporterEntity == null)
                {
                    return true;
                }
                WaferHolderInfo waferHolderInfo = transporterEntity.WaferHolderInfo;
                if (waferHolderInfo == null)
                {
                    return true;
                }
                if (waferHolderInfo.Id == str)
                {
                    return true;
                }
                WaferHolderManager.Instance.SwitchWaferHolderId(waferHolderInfo,Module,str);
                return true;
            }
            return true;
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _cellName = objs[0].ToString();
            _elevatorAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Elevator");;
            _gantryAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Gantry");
            _loaderEntity = Singleton<RouteManager>.Instance.GetModule<LoaderEntity>(ModuleName.Loader1.ToString());
            _loaderRotationAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.Loader1}.Rotation");
            _conflictRoutine = new TransporterConflictRoutine(Module);
            _transporterCommon = DEVICE.GetDevice<TransporterCommon>($"{Module}.Common");
            _loaderCommonDevice = DEVICE.GetDevice<LoaderCommonDevice>($"{ModuleName.Loader1}.Common");
            _bypassWaferHolderPresent = SC.GetValue<bool>("Transporter.BypassWaferHolderPresent");
            _velocity = 0;
            _acceleration = 0;
            _pickMaxRetries = SC.GetValue<int>("Transporter.MaxPickTries");
            string preConfig = SC.GetConfigPreContent(_cellName);
            if (SC.ContainsItem($"{preConfig}.PickTimeSeconds"))
            {
                _pickupTime = SC.GetValue<int>($"{preConfig}.PickTimeSeconds");
            }
            if (SC.ContainsItem($"{preConfig}.PickDelaySeconds"))
            {
                _pickupDelayTime = SC.GetValue<int>($"{preConfig}.PickDelaySeconds");
            }
            return Runner.Start(Module, $"PickUpFrom {_cellName}");
        }
        
        /// <summary>
        /// 启动校验条件
        /// </summary>
        /// <returns></returns>
        private bool CheckStartPreConfition()
        {
            //所有轴上电并Homed
            bool result = CheckPreCondition();
            if(!result)
            {
                return false;
            }        
            //Transporter的Wafer shuttle hold present信号为off
            if (!_bypassWaferHolderPresent && _transporterCommon.TransporterData.WSHoldPresent)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} WaferShuttleHoldPresent is on", -1);
                return false;
            }
            //Loader is Home
            if (!_loaderEntity.IsHomed)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{ModuleName.Loader1} is not homed", -1);
                return false;
            }
            //若目标Cell为Loader, 则Loader需在TRNA或TRANB位置且WaferShuttlePresent信号on
            if (_cellName == "Loader")
            {
                double loaderRotationPosition = _loaderRotationAxis.MotionData.MotorPosition;
                if (!_loaderRotationAxis.CheckPositionIsInStation(loaderRotationPosition, "TRNPA") && 
                    !_loaderRotationAxis.CheckPositionIsInStation(loaderRotationPosition, "TRNPB"))
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, $"{ModuleName.Loader1} rotation axis {loaderRotationPosition} is not int TRNPA or TRNPB station",-1);
                    return false;
                }
                if(!_loaderCommonDevice.CommonData.WaferHolderPresent)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, $"{ModuleName.Loader1} WaferShuttlePresent is off", -1);
                    return false;
                }
            }
            //Transporter没有WS信息
            if (WaferHolderManager.Instance.HasWaferHolder(Module))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} already has wafer Shuttle", -1);
                return false;
            }
            //目标Cell应有WS信息
            if (!WaferHolderManager.Instance.HasWaferHolder(_cellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{_cellName} does not has wafer shuttle", -1);
                return false;
            }

            //检验Facilities
            //var faciltiesResult = _facilities.CheckCDA();
            //if (!faciltiesResult.result)
            //{
            //    NotifyError(eEvent.ERR_TRANSPORTER, faciltiesResult.reason, -1);
            //    return false;
            //}
            return true;
        }
        /// <summary>
        /// 检验前置条件
        /// </summary>
        /// <returns></returns>
        private bool CheckPreCondition()
        {
            if (!_gantryAxis.IsSwitchOn)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} gantry axis is not switch on ", -1);
                return false;
            }
            if (!_gantryAxis.IsHomed)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} gantry axis is not homed ",-1);
                return false;
            }
            if (!_elevatorAxis.IsSwitchOn)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} elevator axis is not switch on ", -1);
                return false;
            }
            if (!_elevatorAxis.IsHomed)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} elevator axis is not homed ",-1);
                return false;
            }


            return true;
        }
    }
}