CyberX8/CyberX8_RT/Modules/Transporter/TransporterMoveToRoutine.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.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 TransporterMoveToRoutine : RoutineBase, IRoutine
    {
        private enum MoveToStep
        {
            CheckStatus,
            ElevatorPosition,
            ElevatorPositionWait,
            ImmobilizeExtend,
            ImmobilizeExtendWait,
            SafeMoveTo,
            CheckMoveToStatus,
            GantryPosition,
            GantryPositionWait,
            End
        }

        #region 内部变量
        private string _cellName;
        private JetAxisBase _gantryAxis;
        private JetAxisBase _elevatorAxis;
        private LoaderEntity _loaderEntity;
        private JetAxisBase _loaderRotationAxis;
        private TransporterCommon _transporterCommon;
        private SystemFacilities _facilities;
        private TransporterExtendRoutine _extendRoutine;
        private TransporterConflictRoutine _conflictRoutine;
        ProcessLayoutCellItem _cellItem;
        private bool _waferHolderPresent = false;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public TransporterMoveToRoutine(string module) : base(module)
        {
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(MoveToStep.CheckStatus, CheckStartPreConfition, NullFun,_delay_1ms)
                //1.1 Elevator
                .Run(MoveToStep.ElevatorPosition, ElevatorPosition, _delay_1ms)
                .WaitWithStopCondition(MoveToStep.ElevatorPositionWait, CheckVerticalPositionStatus, CheckVerticalPositionRunStop)
                //1.1 Immobilizer extend
                .Run(MoveToStep.ImmobilizeExtend, ImmobilizeExtend, _delay_1ms)
                .WaitWithStopCondition(MoveToStep.ImmobilizeExtendWait, CheckImmobilizeExtendStatus, CheckImmobilizeExtendRunStop)
                //1.2 Move to 安全
                .Run(MoveToStep.SafeMoveTo,SafeMoveTo,_delay_1ms)
                .WaitWithStopCondition(MoveToStep.CheckMoveToStatus, () => CommonFunction.CheckRoutineEndState(_conflictRoutine),
                    CheckSafeMoveToStopStatus)
                //1.3 Gantry 移动
                .Run(MoveToStep.GantryPosition, GantryPositionToCell, _delay_1ms)
                .WaitWithStopCondition(MoveToStep.GantryPositionWait, CheckGantryPositionStatus, CheckGantryPositionRunStop)
                .End(MoveToStep.End,NullFun,100);
            return Runner.Status;
        }
        /// <summary>
        /// Elevator Position
        /// </summary>
        /// <returns></returns>
        private bool ElevatorPosition()
        {
            bool result= false;
            //状态locked住了 则运动至Up
            if(_waferHolderPresent)
            {
                result= _elevatorAxis.PositionStation("Up");
                if(!result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "elevator goto up failed", 0);
                }
            }
            else//不存在Wafer,则运动至Low
            {
                result= _elevatorAxis.PositionStation("LOW");
                if (!result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "elevator goto low failed", 0);
                }
            }
            return result;
        }
        /// <summary>
        /// Immobile Extend
        /// </summary>
        /// <returns></returns>
        private bool ImmobilizeExtend()
        {
            if(!_waferHolderPresent)
            {
                return true;
            }
            else
            {
                return _extendRoutine.Start(null) == RState.Running;
            }
        }
        /// <summary>
        /// 检验Immobilize状态
        /// </summary>
        /// <returns></returns>
        private bool CheckImmobilizeExtendStatus()
        {
            if(!_waferHolderPresent)
            {
                return true;
            }
            else
            {
                return CommonFunction.CheckRoutineEndState(_extendRoutine);
            }
        }
        /// <summary>
        /// 检验Immobilize状态
        /// </summary>
        /// <returns></returns>
        private bool CheckImmobilizeExtendRunStop()
        {
            if (!_waferHolderPresent)
            {
                return true;
            }
            else
            {
                bool result= CommonFunction.CheckRoutineStopState(_extendRoutine);
                if(result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "extend failed", 0);
                }
                return result;
            }
        }
        /// <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>
        /// 检验Vertical移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckVerticalPositionStatus()
        {
            return _elevatorAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Vertical是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckVerticalPositionRunStop()
        {
            bool result= _elevatorAxis.Status == RState.Failed||_elevatorAxis.Status==RState.Timeout;
            if (result)
            {
                NotifyError(eEvent.ERR_TRANSPORTER, "elevator 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>
        /// <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");
            _facilities = DEVICE.GetDevice<SystemFacilities>("System.Facilities");
            _extendRoutine = new TransporterExtendRoutine(Module);
            _conflictRoutine = new TransporterConflictRoutine(Module);
            _transporterCommon = DEVICE.GetDevice<TransporterCommon>($"{Module}.Common");
            _waferHolderPresent = _transporterCommon.TransporterData.WhPresent1||_transporterCommon.TransporterData.WhPresent2;
            return Runner.Start(Module,$"MoveTo {_cellName}");
        }
        /// <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 (_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 (WaferHolderManager.Instance.HasWaferHolder(Module)&& !_elevatorAxis.CheckPositionIsInStation(elevatorPosition, "Up"))
            {
                NotifyError(eEvent.ERR_TRANSPORTER,$"{Module} has wafer Shuttle but elevator axis is not in up",-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;
        }
    }
}