CyberX8/CyberX8_RT/Modules/Transporter/TransporterTransferRoutine.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.SubstrateTrackings;
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.TransPorter;
using CyberX8_RT.Modules.Loader;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Aitex.Core.RT.Fsm;

namespace CyberX8_RT.Modules.Transporter
{
   
    public class TransporterTransferRoutine : RoutineBase, IRoutine
    {
        private enum TransferStep
        {
            CheckPreCondition,
            PickUpFrom,
            PickUpFromWait,
            MoveTo,
            MoveToWait,
            Place,
            PlaceWait,
            End
        }

        #region 内部变量
        private string _fromCellName;
        private string _toCellName;
        private JetAxisBase _gantryAxis;
        private JetAxisBase _elevatorAxis;
        private LoaderEntity _loaderEntity;
        private JetAxisBase _loaderRotationAxis;
        private SystemFacilities _facilities;
        private TransporterPickUpFromRoutine _pickUpRoutine;
        private TransporterMoveToRoutine _moveToRoutine;
        private TransporterPickDownToRoutine _placeRoutine;
        #endregion

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public TransporterTransferRoutine(string module) : base(module)
        {
            _pickUpRoutine=new TransporterPickUpFromRoutine(module);
            _moveToRoutine = new TransporterMoveToRoutine(module);
            _placeRoutine=new TransporterPickDownToRoutine(module);
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(TransferStep.CheckPreCondition,CheckStartPreConfition,_delay_1ms)
            //1.1 PickupFrom
                .Run(TransferStep.PickUpFrom, () => { return _pickUpRoutine.Start(_fromCellName) == RState.Running; }, _delay_1ms)
                .WaitWithStopCondition(TransferStep.PickUpFromWait, ()=>CommonFunction.CheckRoutineEndState(_pickUpRoutine), 
               ()=> CheckRoutineStopStatus(_pickUpRoutine,"PickUp Routine", _pickUpRoutine.ErrorMsg,0))
                //1.2 Move to
                .Run(TransferStep.MoveTo, () => { return _moveToRoutine.Start(_toCellName) == RState.Running; }, _delay_1ms)
                .WaitWithStopCondition(TransferStep.MoveToWait, ()=>CommonFunction.CheckRoutineEndState(_moveToRoutine),
                ()=> CheckRoutineStopStatus(_moveToRoutine,"MoveTo Routine",_moveToRoutine.ErrorMsg,1))
                //1.3 Place
                .Run(TransferStep.Place, () => { return _placeRoutine.Start(_toCellName) == RState.Running; },_delay_1ms)
                .WaitWithStopCondition(TransferStep.PlaceWait, () => CommonFunction.CheckRoutineEndState(_placeRoutine), 
                ()=> CheckRoutineStopStatus(_placeRoutine,"Place Routine",_placeRoutine.ErrorMsg,2))
                .End(TransferStep.End,LastChangeWafer,100);
            return Runner.Status;
        }
        /// <summary>
        /// 检验Routine异常结束状态
        /// </summary>
        /// <param name="routine"></param>
        /// <returns></returns>
        private bool CheckRoutineStopStatus(IRoutine routine,string routineName,string errorMsg,int step)
        {
            RState ret = routine.Monitor();
            if (ret == RState.Failed || ret == RState.Timeout)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"Transfer WaferHolder Routine\r\n{routineName}\\r\n{errorMsg}",step);
                return true;
            }
            return false;
        }

        /// <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>
        /// 最后交换Wafer
        /// </summary>
        /// <returns></returns>
        private bool LastChangeWafer()
        {
            ModuleName sourceModule;
            if (_fromCellName.ToLower() == "loader")
            {
                sourceModule = ModuleName.Loader1;
            }
            else
            {
                sourceModule = (ModuleName)Enum.Parse(typeof(ModuleName), _fromCellName);
            }
            ModuleName destModule;
            if (_toCellName.ToLower() == "loader")
            {
                destModule = ModuleName.Loader1;
            }
            else
            {
                destModule = (ModuleName)Enum.Parse(typeof(ModuleName), _toCellName);
            }
            if (WaferManager.Instance.CheckHasWafer(sourceModule, 0))
            {
                WaferManager.Instance.WaferMoved(sourceModule, 0, destModule, 0);
            }
            if (WaferManager.Instance.CheckHasWafer(sourceModule, 1))
            {
                WaferManager.Instance.WaferMoved(sourceModule, 1, destModule, 1);
            }
            return true;
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _fromCellName = objs[0].ToString();
            _toCellName = objs[1].ToString();
            InitializeParameters();
            return Runner.Start(Module, $"Transfer from {_fromCellName} to {_toCellName}");
        }
        /// <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");
        }
        /// <summary>
        /// 启动校验条件
        /// </summary>
        /// <returns></returns>
        private bool CheckStartPreConfition()
        {
            if(!CheckPreCondition())
            {
                return false;
            }
            double elevatorPosition = _elevatorAxis.MotionData.MotorPosition;
            if (!_elevatorAxis.CheckPositionIsInStation(elevatorPosition, "Up") && 
                !_elevatorAxis.CheckPositionIsInStation(elevatorPosition,"Low"))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} elevator axis {elevatorPosition} is not in Up or Low station",-1);
                return false;
            }

            if (!_loaderEntity.IsHomed)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{ModuleName.Loader1} is not homed", -1);
                return false;
            }
            if (_toCellName == "Loader" || _fromCellName == "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 (WaferHolderManager.Instance.HasWaferHolder(Module))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} has wafer shuttle", -1);
                return false;
            }
            if (!WaferHolderManager.Instance.HasWaferHolder(_fromCellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER,  $"{_fromCellName} does not has wafer shuttle", -1);
                return false;
            }
            if (WaferHolderManager.Instance.HasWaferHolder(_toCellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{_toCellName} already 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>
        /// <param name="step"></param>
        public RState Retry(int step)
        {
            InitializeParameters();
            if(string.IsNullOrEmpty(_fromCellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "source cell is empty", step);
                return RState.Failed;
            }
            if (string.IsNullOrEmpty(_toCellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "target cell is empty", step);
                return RState.Failed;
            }

            List<Enum> preStepIds = new List<Enum>();
            if (step == 0 || step == -1)
            {
                return Runner.Retry(TransferStep.CheckPreCondition, preStepIds, Module, "Transfer Retry");
            }
            else if(step==1)
            {
                AddPreSteps(TransferStep.MoveTo, preStepIds);
                return Runner.Retry(TransferStep.MoveTo, preStepIds, Module, $"Transfer step {TransferStep.MoveTo} Retry");
            }
            else 
            {
                AddPreSteps(TransferStep.Place, preStepIds);
                return Runner.Retry(TransferStep.Place, preStepIds, Module, $"Transfer step {TransferStep.Place} Retry");
            }
        }
        /// <summary>
        /// 忽略前
        /// </summary>
        /// <param name="step"></param>
        /// <param name="preStepIds"></param>
        private void AddPreSteps(TransferStep step, List<Enum> preStepIds)
        {
            for (int i = 0; i < (int)step; i++)
            {
                preStepIds.Add((TransferStep)i);
            }
        }

        /// <summary>
        /// 检验前面完成状态
        /// </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(_toCellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{_toCellName} does not have waferholder", index);
                return false;
            }
            double gantryPosition = _gantryAxis.MotionData.MotorPosition;
            if (!_gantryAxis.CheckPositionIsInStation(gantryPosition, _toCellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} gantry {gantryPosition} not in {_toCellName}", index);
                return false;
            }
            return true;
        }
    }
}