CyberX8/CyberX8_RT/Modules/Transporter/TransporterPickDownToRoutine.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.Metal;
using CyberX8_RT.Devices.Rinse;
using static Mono.Security.X509.X520;

namespace CyberX8_RT.Modules.Transporter
{
   
    public class TransporterPickDownToRoutine : RoutineBase, IRoutine
    {
        private enum PutDownStep
        {
            CheckPreStatus,
            ElevatorUp,
            ElevatorUpWait,
            SafeMoveTo,
            CheckMoveToStatus,
            GantryPosition,
            GantryPositiolWait,
            ElevatorPositionToCellTop,
            ElevatorPositionToCellTopWait,
            Delay,
            CalculatePlaceSpeed,
            PlaceDelay,            
            ElevatorPositionToCell,
            ElevatorPositionToCellWait,
            DropBlockLockoff,
            ElevatorGotoUp,
            ElevatorGotoUpWait,         
            ConfirmWSPresent,
            UpdateWaferHolder,
            GantryPositionPark,
            GantryPositionParkWait,
            End            
        }

        #region 内部变量
        private string _cellName;
        private string _otherModule;
        private JetAxisBase _gantryAxis;
        private JetAxisBase _elevatorAxis;
        private JetAxisBase _otherElevatorAxis;
        private LoaderEntity _loaderEntity;
        private JetAxisBase _loaderRotationAxis;
        private SystemFacilities _facilities;
        private TransporterCommon _transporterCommon;
        private TransporterConflictRoutine _conflictRoutine;
        private LoaderCommonDevice _loaderCommonDevice;
        ProcessLayoutCellItem _cellItem;
        private int _placeTime;
        private int _placeDelayTime;
        private int _velocity;
        private int _acceleration;
        private bool _bypassWaferHolderPresent;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public TransporterPickDownToRoutine(string module) : base(module)
        {
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(PutDownStep.CheckPreStatus,CheckStartPreConfition,_delay_1ms)
                //1. Elevator 移动至Up
                .Run(PutDownStep.ElevatorUp, ElevatorGotoUp, _delay_1ms)
                .WaitWithStopCondition(PutDownStep.ElevatorUpWait, CheckElevatorPositionEndStatus, CheckElevatorPositionStopStatus)
                //2. other Transporter Safe Move 
                .Run(PutDownStep.SafeMoveTo, SafeMoveTo, _delay_1ms)
                .WaitWithStopCondition(PutDownStep.CheckMoveToStatus, () => CommonFunction.CheckRoutineEndState(_conflictRoutine),
                    CheckSafeMoveToStopStatus)
                //3. Gantry 移动
                .Run(PutDownStep.GantryPosition, GantryPositionToCell, _delay_1ms)
                .WaitWithStopCondition(PutDownStep.GantryPositiolWait, CheckGantryPositionStatus, CheckGantryPositionRunStop)                      
                //6. Elevator 移动至cellTop
                .Run(PutDownStep.ElevatorPositionToCellTop, ElevatorPositionToCellTop, _delay_1ms)
                .WaitWithStopCondition(PutDownStep.ElevatorPositionToCellTopWait, CheckElevatorPositionEndStatus, CheckElevatorPositionStopStatus)
                .Delay(PutDownStep.Delay, 500)
                //4. 确认Place 速度和加速度
                .Run(PutDownStep.CalculatePlaceSpeed, CalculatePownDownSpeed, _delay_1ms)
                //5. Pickup delay
                .Delay(PutDownStep.PlaceDelay, _placeDelayTime * 1000)
                //7. Elevator 移动至cell
                .Run(PutDownStep.ElevatorPositionToCell, ElevatorPositionToCell, _delay_1ms)
                .WaitWithStopCondition(PutDownStep.ElevatorPositionToCellWait, CheckElevatorPositionEndStatus, CheckElevatorPositionStopStatus)
                //8. DropBlockLock off
                .Run(PutDownStep.DropBlockLockoff, DropBlockLockoff , _delay_1ms)
                //9. Elevator移动至Up
                .Run(PutDownStep.ElevatorGotoUp, ElevatorGotoUp, _delay_1ms)
                .WaitWithStopCondition(PutDownStep.ElevatorGotoUpWait, CheckElevatorPositionEndStatus, CheckElevatorPositionStopStatus)
                //10. 确认WSPresent信号
                .Wait(PutDownStep.ConfirmWSPresent,ConfirmWSPresentSensorOff,_delay_2s)                
                //11. Wafer Holder location update
                .Run(PutDownStep.UpdateWaferHolder,WaferHolderTransfer,_delay_1ms)
                //12. Gantry Position To Park
                .Run(PutDownStep.GantryPositionPark, GantryBackToPark, _delay_1ms)
                .WaitWithStopCondition(PutDownStep.GantryPositionParkWait, CheckGantryParkPositionStatus, CheckGantryParkPositionRunStop)
                .End(PutDownStep.End,NullFun,100);
            return Runner.Status;
        }       
        /// <summary>
        /// Elevator goto Up
        /// </summary>
        /// <returns></returns>
        private bool ElevatorGotoUp()
        {
            bool result= _elevatorAxis.PositionStation("UP", false);
            if (!result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "elevator goto up failed", 0);
            }
            return result;
        }
        /// <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;
        }
        /// <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 cellTop
        /// </summary>
        /// <returns></returns>
        private bool ElevatorPositionToCellTop()
        {
            bool result = _elevatorAxis.PositionStation("CellTop", false, _velocity, _acceleration, _acceleration);
            if (!result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "elevator goto CellTop failed", 0);
            }
            return result;
        }
        /// <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,_velocity,_acceleration,_acceleration);
                }
                else
                {
                    result= _elevatorAxis.PositionStation(_cellName,false,_velocity,_acceleration,_acceleration);
                }
                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>
        /// 检验Elevator移动状态
        /// </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 axis motion failed", 0);
            }
            return result;
        }
        /// <summary>
        /// 计算放下速度
        /// </summary>
        /// <returns></returns>
        private bool CalculatePownDownSpeed()
        {
            if(_placeTime==0)
            {
                return true;
            }  
            BeckhoffStationAxis stationAxis = BeckhoffStationLocationManager.Instance.GetStationAxis(Module, "Elevator", 0);
            Station upStation = stationAxis.Stations.Find(O => O.Name.EndsWith("CellTop")); 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, "CellTop 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/ _placeTime;
            _velocity = _elevatorAxis.CalculateMultiplySpeedRatio(_elevatorAxis.CalculateValueMultiplyScale(tmpVelocity));
            double tmpAccelaration = (double)(2 * distance) / Math.Pow(_placeTime, 2);
            _acceleration = _elevatorAxis.CalculateMultiplyAccelerationRatio(_elevatorAxis.CalculateValueMultiplyScale(tmpAccelaration));
            LOG.WriteBackgroundLog(eEvent.INFO_TRANSPORTER, Module, "adjust profile speed");
            return true;
        }      
        /// <summary>
        /// 更新WaferHolder移动信息
        /// </summary>
        /// <returns></returns>
        private bool WaferHolderTransfer()
        {
            bool result= WaferHolderManager.Instance.TransferWaferHolder(Module,Module, _cellName);
            if (!result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "tranfer waferHolder message failed", 0);
            }
            else
            {
                if (Singleton<RouteManager>.Instance.IsAutoRunning)
                {
                    WaferHolderInfo waferHolderInfo = WaferHolderManager.Instance.GetWaferHolder(_cellName);
                    if (waferHolderInfo != null)
                    {
                        string strTime = DateTime.Now.ToString("yyyy/MM/dd HH:mm:ss");
                        waferHolderInfo.SchedulerModules.Add($"{strTime} {Module} => {_cellName}");
                    }
                }
            }
            return result;
        }
        /// <summary>
        /// 确认WH Present Sensor off
        /// </summary>
        /// <returns></returns>
        private bool ConfirmWSPresentSensorOff()
        {
            if (_bypassWaferHolderPresent) return true;
            bool result= !_transporterCommon.TransporterData.WSHoldPresent;
            if(!result)
            {
                LOG.WriteLog(eEvent.INFO_TRANSPORTER, Module, "confirm WS Present sensor failed");
            }
            return result;
        }
       
        /// <summary>
        /// Gabtry是否需要回到
        /// </summary>
        /// <returns></returns>
        private bool NeedGantryBackToPark()
        {
            switch(_cellName)
            {
                case "Loader":
                case "Prewet":
                    return true;
                default:
                    return false;
            }
        }
        /// <summary>
        /// Gantry 回至Park
        /// </summary>
        /// <returns></returns>
        private bool GantryBackToPark()
        {
            if (NeedGantryBackToPark())
            {
                bool result= _gantryAxis.PositionStation("Park", false);
                if(!result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "gantry back to park failed",0);
                }
                return result;
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验Gantry Park移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckGantryParkPositionStatus()
        {
            if (NeedGantryBackToPark())
            {
                return _gantryAxis.Status == RState.End;
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验Gantry Park是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckGantryParkPositionRunStop()
        {
            if (NeedGantryBackToPark())
            {
                bool result = _gantryAxis.Status == RState.Failed||_gantryAxis.Status==RState.Timeout;
                if(result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "gantry motion failed", 0);
                }
                return result;
            }
            else
            {
                return false;
            }
        }
        /// <summary>
        /// 关闭DropBlockLock
        /// </summary>
        /// <returns></returns>
        private bool DropBlockLockoff()
        {
            if (_transporterCommon.TransporterData.Lock)
            {
                return _transporterCommon.UnlockOperation("", null);
            }
            return true;
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _cellName = objs[0].ToString();            
            _velocity = 0;
            _acceleration = 0;
            InitializeParameters();
            string preConfig = SC.GetConfigPreContent(_cellName);
            _bypassWaferHolderPresent = SC.GetValue<bool>("Transporter.BypassWaferHolderPresent");
            if (SC.ContainsItem($"{preConfig}.PlaceTimeSeconds"))
            {
                _placeTime = SC.GetValue<int>($"{preConfig}.PlaceTimeSeconds");
            }
            if (SC.ContainsItem($"{preConfig}.PlaceDelaySeconds"))
            {
                _placeDelayTime = SC.GetValue<int>($"{preConfig}.PlaceDelaySeconds");
            }

            return Runner.Start(Module, $"Pun Down To {_cellName}");
        }
        /// <summary>
        /// 初始化参数
        /// </summary>
        private void InitializeParameters()
        {
            _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");
            _facilities = DEVICE.GetDevice<SystemFacilities>("System.Facilities");
            _conflictRoutine = new TransporterConflictRoutine(Module);
            _transporterCommon = DEVICE.GetDevice<TransporterCommon>($"{Module}.Common");
            _otherModule = Module == "Transporter2" ? "Transporter1" : "Transporter2";
            _otherElevatorAxis = DEVICE.GetDevice<JetAxisBase>($"{_otherModule}.Elevator");
            _loaderCommonDevice = DEVICE.GetDevice<LoaderCommonDevice>($"{ModuleName.Loader1}.Common");
        }
        /// <summary>
        /// 启动校验条件
        /// </summary>
        /// <returns></returns>
        private bool CheckStartPreConfition()
        {
            //所有轴上电并Homed
            bool result = CheckPreCondition();
            if(!result)
            {
                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 on", -1);
                    return false;
                }
            }
            //Transporter应有WS信息
            if (!WaferHolderManager.Instance.HasWaferHolder(Module))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} does not has wafer Shuttle",-1);
                return false;
            }
            //目标Cell没有WS信息
            if (WaferHolderManager.Instance.HasWaferHolder(_cellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"Cell {_cellName} already has wafer Shuttle", -1);
                return false;
            }         
            //检验Lock
            if (!_transporterCommon.TransporterData.Lock)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} Drop block lock is off ", -1);
                return false;
            }
            //检验Wafer shuttle hold present
            if (!_bypassWaferHolderPresent && !_transporterCommon.TransporterData.WSHoldPresent)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} Wafer shuttle hold present sensor is off ",-1);
                return false;
            }

            //检验Facilities
            //var faciltiesResult = _facilities.CheckCDA();
            //if (!faciltiesResult.result)
            //{
            //    NotifyError(eEvent.ERR_TRANSPORTER, faciltiesResult.reason, -1);
            //    return false;
            //}
            return true;
        }
        /// <summary>
        /// 重试
        /// </summary>
        /// <param name="step"></param>
        public RState Retry(int step)
        {
            InitializeParameters();
            List<Enum> preStepIds = new List<Enum>();
            return Runner.Retry(PutDownStep.CheckPreStatus, preStepIds, Module, "PickUp Moveto Retry");
        }
        /// <summary>
        /// 检验前面Unload完成状态
        /// </summary>
        /// <returns></returns>
        public bool CheckCompleteCondition(int index)
        {
            TransporterEntity transporterEntity = Singleton<RouteManager>.Instance.GetModule<TransporterEntity>(Module);
            if (transporterEntity.WaferHolderInfo != null)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} has waferholder", index);
                return false;
            }
            if (!WaferHolderManager.Instance.HasWaferHolder(_cellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{_cellName} does not have waferholder", index);
                return false;
            }
            return true;
        }
    }
}