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,
            SafeMoveTo,
            CheckMoveToStatus,
            GantryPosition,
            GantryPositionWait,
            End
        }

        #region 内部变量
        private string _cellName;
        private string _otherModule;
        private JetAxisBase _gantryAxis;
        private JetAxisBase _elevatorAxis;
        private JetAxisBase _otherElevatorAxis;
        private LoaderEntity _loaderEntity;
        private JetAxisBase _loaderRotationAxis;
        private TransporterCommon _transporterCommon;
        private SystemFacilities _facilities;
        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. Elevator
                .Run(MoveToStep.ElevatorPosition, ElevatorPosition, _delay_1ms)
                .WaitWithStopCondition(MoveToStep.ElevatorPositionWait, CheckElevatorPositionStatus, CheckElevatorPositionRunStop)
                //2. other Transporter Safe Move
                .Run(MoveToStep.SafeMoveTo, SafeMoveTo, _delay_1ms)
                .WaitWithStopCondition(MoveToStep.CheckMoveToStatus, () => CommonFunction.CheckRoutineEndState(_conflictRoutine),
                    CheckSafeMoveToStopStatus)
                //3. this Gantry Move To Target Cell
                .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;
            //运动至UP
            double elevatorPosition = _elevatorAxis.MotionData.MotorPosition;
            if (!_elevatorAxis.CheckPositionIsInStation(elevatorPosition, "UP"))
            {
                result= _elevatorAxis.PositionStation("UP");
                if(!result)
                {
                    NotifyError(eEvent.ERR_TRANSPORTER, "elevator goto up failed", 0);
                }
            }
            else
            {
                result = true;
            }          
            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>
        /// 检验Elevator移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckElevatorPositionStatus()
        {
            return _elevatorAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Elevator是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckElevatorPositionRunStop()
        {
            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");
            _otherModule = Module == "Transporter2" ? "Transporter1" : "Transporter2";
            _otherElevatorAxis = DEVICE.GetDevice<JetAxisBase>($"{_otherModule}.Elevator");
            _gantryAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Gantry");
            _loaderEntity = Singleton<RouteManager>.Instance.GetModule<LoaderEntity>(ModuleName.Loader1.ToString());
            _loaderRotationAxis = DEVICE.GetDevice<JetAxisBase>($"{ModuleName.Loader1}.Rotation");
            _conflictRoutine = new TransporterConflictRoutine(Module);
            _transporterCommon = DEVICE.GetDevice<TransporterCommon>($"{Module}.Common");
            return Runner.Start(Module,$"MoveTo {_cellName}");
        }
        /// <summary>
        /// 启动校验条件
        /// </summary>
        /// <returns></returns>
        private bool CheckStartPreConfition()
        {
            //所有轴上电并Homed
            if(!CheckPreCondition())
            {
                return false;
            }            

            //若目标Cell为Loader, 则Loader应在TRNPA或TRNPB位
            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;
                }
            }

            //检验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;
        }
    }
}