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

        #region 内部变量
        private string _cellName;
        private JetAxisBase _gantryAxis;
        private JetAxisBase _elevatorAxis;
        private LoaderEntity _loaderEntity;
        private JetAxisBase _loaderRotationAxis;
        private SystemFacilities _facilities;
        private TransporterConflictRoutine _conflictRoutine;
        ProcessLayoutCellItem _cellItem;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public TransporterGantryPositionRoutine(string module) : base(module)
        {
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(MoveToStep.SafeMoveTo,SafeMoveTo,_delay_1ms)
                .WaitWithStopCondition(MoveToStep.CheckMoveToStatus, () => CommonFunction.CheckRoutineEndState(_conflictRoutine), () => CommonFunction.CheckRoutineStopState(_conflictRoutine))
                //1.3 Gantry 移动
                .Run(MoveToStep.GantryPosition, GantryPositionToCell, NullFun, _delay_1ms)
                .WaitWithStopCondition(MoveToStep.GantryPositionWait, CheckGantryPositionStatus, CheckGantryPositionRunStop)
                .End(MoveToStep.End,NullFun,100);
            return Runner.Status;
        }
        /// <summary>
        /// 安全避障移动
        /// </summary>
        /// <returns></returns>
        private bool SafeMoveTo()
        {
            string stationName = GetStationName();
            if (string.IsNullOrEmpty(stationName))
            {
                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
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{_cellName} not in station list");
                return false;
            }
        }
        /// <summary>
        /// 根据cellName获取StationName
        /// </summary>
        /// <returns></returns>
        private string GetStationName()
        {
            string stationName = _cellName;
            if (_cellName != "Park" && _cellName != "Loader")
            {
                _cellItem = ProcessLayoutManager.Instance.GetProcessLayoutCellItemByName(_cellName);
                if (_cellItem != null)
                {
                    stationName = $"Cell{_cellItem.CellId}";
                }
                else
                {
                    LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{_cellName} not in layout");
                    return "";
                }
            }
            return stationName;
        }
        /// <summary>
        /// Gantry Position To cell
        /// </summary>
        /// <returns></returns>
        private bool GantryPositionToCell()
        {
            string stationName = GetStationName();
            if (string.IsNullOrEmpty(stationName))
            {
                return false;
            }
            return _gantryAxis.PositionStation(stationName);
        }
        /// <summary>
        /// 检验Gantry移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckGantryPositionStatus()
        {
            return _gantryAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Gantry是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckGantryPositionRunStop()
        {
            return _gantryAxis.Status == RState.Failed;
        }
        /// <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");
            _conflictRoutine = new TransporterConflictRoutine(Module);
            if(!CheckStartPreConfition())
            {
                return RState.Failed;
            }
            return Runner.Start(Module, $"GantryGoToPosition {_cellName}");
        }
        /// <summary>
        /// 启动校验条件
        /// </summary>
        /// <returns></returns>
        private bool CheckStartPreConfition()
        {            
            if (!_loaderEntity.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{ModuleName.Loader1} is not homed");
                return false;
            }
            if (_cellName == "Loader")
            {
                double loaderRotationPosition = _loaderRotationAxis.MotionData.MotorPosition;
                if (!_loaderRotationAxis.CheckPositionIsInStation(loaderRotationPosition, "TRNPA") && 
                    !_loaderRotationAxis.CheckPositionIsInStation(loaderRotationPosition, "TRNPB"))
                {
                    LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{ModuleName.Loader1} rotation axis {loaderRotationPosition} is not int TRNPA or TRNPB station");
                    return false;
                }
            }
            string otherTransporter = Module == ModuleName.Transporter2.ToString() ? ModuleName.Transporter1.ToString() : ModuleName.Transporter1.ToString();
            TransporterEntity otherTransporterEntity = Singleton<RouteManager>.Instance.GetModule<TransporterEntity>(otherTransporter);
            if (!otherTransporterEntity.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{otherTransporter} is not homed");
                return false;
            }

            if (otherTransporterEntity.IsBusy)
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{otherTransporter} is busy");
                return false;
            }
            double elevatorPosition=_elevatorAxis.MotionData.MotorPosition;
            if (WaferHolderManager.Instance.HasWaferHolder(Module)&& !_elevatorAxis.CheckPositionIsInStation(elevatorPosition, "Up"))
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{Module} has wafer Shuttle but elevator axis is not in up");
                return false;
            }
            
            if (!_gantryAxis.IsSwitchOn)
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{Module} gantry axis is not switch on ");
                return false;
            }
            if (!_gantryAxis.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{Module} gantry axis is not homed ");
                return false;
            }
            if (!_elevatorAxis.IsSwitchOn)
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{Module} elevator axis is not switch on ");
                return false;
            }
            if (!_elevatorAxis.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_TRANSPORTER, Module, $"{Module} elevator axis is not homed ");
                return false;
            }
            return true;
        }
    }
}