CyberX8/CyberX8_RT/Modules/Transporter/TransporterPickUpMoveToRoutine.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;

namespace CyberX8_RT.Modules.Transporter
{
   
    public class TransporterPickUpMoveToRoutine : RoutineBase, IRoutine
    {
        private enum TransferStep
        {
            CheckStatus,
            PickUpFrom,
            PickUpFromWait,
            MoveTo,
            MoveToWait,
            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;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public TransporterPickUpMoveToRoutine(string module) : base(module)
        {
            _pickUpRoutine=new TransporterPickUpFromRoutine(module);
            _moveToRoutine = new TransporterMoveToRoutine(module);
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(TransferStep.CheckStatus, CheckStartPreConfition,_delay_1ms)
                //1.1 PickupFrom
                .Run(TransferStep.PickUpFrom, () => { return _pickUpRoutine.Start(_fromCellName) == RState.Running; }, _delay_1ms)
                .WaitWithStopCondition(TransferStep.PickUpFromWait, ()=>CommonFunction.CheckRoutineEndState(_pickUpRoutine), CheckPickupStopStatus)
                //1.2 Move to
                .Run(TransferStep.MoveTo, () => { return _moveToRoutine.Start(_toCellName) == RState.Running; }, _delay_1ms)
                .WaitWithStopCondition(TransferStep.MoveToWait, ()=>CommonFunction.CheckRoutineEndState(_moveToRoutine), CheckMoveToStopStatus)
                .End(TransferStep.End,LastChangeWafer,100);
            return Runner.Status;
        }
        /// <summary>
        /// 检验pickup异常状态
        /// </summary>
        /// <returns></returns>
        private bool CheckPickupStopStatus()
        {
            bool result = CommonFunction.CheckRoutineStopState(_pickUpRoutine);
            if (result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"PickUp MoveTo Routine\r\nPickUp Routine\r\n{_pickUpRoutine.ErrorMsg}", 0);
            }
            return result;
        }
        /// <summary>
        /// 检验MoveTo异常状态
        /// </summary>
        /// <returns></returns>
        private bool CheckMoveToStopStatus()
        {
            bool result = CommonFunction.CheckRoutineStopState(_moveToRoutine);
            if (result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"PickUpMoveTo Routine\r\nMoveTo Routine\r\n_moveToRoutine.ErrorMsg", 1);
            }
            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>
        /// 最后交换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, $"Pickup {_fromCellName} Moveto {_toCellName}");
        }
        /// <summary>
        /// 初始化Parameters
        /// </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"))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} elevator axis {elevatorPosition} is no int Up 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)
        {
            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;
            }
            InitializeParameters();
            List<Enum> preStepIds = new List<Enum>();
            if (step == 0 || step == -1)
            {
                return Runner.Retry(TransferStep.CheckStatus, preStepIds, Module, "PickUp Moveto Retry");
            }
            else 
            {
                AddPreSteps(TransferStep.MoveTo, preStepIds);
                return Runner.Retry(TransferStep.MoveTo, preStepIds, Module, $"PickUp Moveto step {TransferStep.MoveTo} 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>
        /// 检验前面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} does not have waferholder", index);
                return false;
            }
            double gantryPosition = _gantryAxis.MotionData.MotorPosition;
            if(!_gantryAxis.CheckPositionIsInStation(gantryPosition,_toCellName))
            {
                NotifyError(eEvent.ERR_TRANSPORTER, $"{Module} gantry not in {_toCellName}", index);
                return false;
            }
            return true;
        }
    }
}