CyberX8/CyberX8_RT/Modules/Transporter/TransporterConflictRoutine.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Routine;
using CyberX8_Core;
using CyberX8_RT.Devices.AXIS;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading.Tasks;
using Aitex.Core.Util;

namespace CyberX8_RT.Modules.Transporter
{
    public class TransporterConflictRoutine : RoutineBase, IRoutine
    {
        private enum ConflictStep
        {
            CheckSafeStatus,
            SafeWaitOtherStatus,
            ReCheckSafeStatus,
            CheckOtherStatus,
            OtherGantryWait,
            End
        }
        #region 内部变量
        private string _otherModule = "";
        private int _transporterMinimumDistance=600;
        private TransporterEntity _otherTransporterEntity;
        private JetAxisBase _otherGantryAxis;
        private double _targetPosition;
        private bool _isOtherStartPosition = false;
        private bool _isOtherGantryChangedToSafeStatus = false;
        /// <summary>
        /// 是否为正向运行
        /// </summary>
        private bool _runPositive = false;
        private bool _isConflict = true;
        private double _safeDistace = 0;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public TransporterConflictRoutine(string module) : base(module)
        {
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(ConflictStep.CheckSafeStatus, CheckSafeConflict,_delay_1ms)
                .WaitWithStopCondition(ConflictStep.SafeWaitOtherStatus, ConflictWaitOtherStatus, () => { return false; })
                .Run(ConflictStep.ReCheckSafeStatus,CheckSafeConflict,_delay_1ms)
                .Run(ConflictStep.CheckOtherStatus, CheckOtherStatus,100)
                .WaitWithStopCondition(ConflictStep.OtherGantryWait, CheckOtherAxisPositionEndStatus, CheckOtherAxisPositionStopStatus)
                .End(ConflictStep.End, NullFun, _delay_1ms);
            return Runner.Status;
        }
        /// <summary>
        /// 检验是否存在安全冲突
        /// </summary>
        /// <returns></returns>
        private bool CheckSafeConflict()
        {
            var result = CheckOtherIsInSafeDistance(_otherGantryAxis.MotionData.MotorPosition);
            if(!result.result)
            {
                _safeDistace = result.safeDistance;
                _isConflict = true;
            }
            return true;
        }
        /// <summary>
        /// 冲突时等待其他Idle状态
        /// </summary>
        /// <returns></returns>
        private bool ConflictWaitOtherStatus()
        {
            if(_isConflict)
            {
                return _otherTransporterEntity.IsIdle;
            }
            return true;
        }
        /// <summary>
        /// 检验另一个Axis状态
        /// </summary>
        /// <returns></returns>
        private bool CheckOtherStatus()
        {           
            if(_isConflict)
            {
                bool positionResult= _otherTransporterEntity.CheckToPostMessage<TransporterState,TransporterMSG>(eEvent.ERR_TRANSPORTER,
                    _otherModule,(int)TransporterMSG.GantrySafeMove,_safeDistace);
                if(positionResult)
                {
                    _isOtherStartPosition = true;
                }
                return positionResult;
            }
            return true;
        }
        /// <summary>
        /// 检验另外Gantry运动情况 
        /// </summary>
        /// <returns></returns>
        private bool CheckOtherAxisPositionEndStatus()
        {
            if(_isOtherStartPosition)
            {
                if (_otherTransporterEntity.State == (int)TransporterState.GantrySafeMoving)
                {
                    _isOtherGantryChangedToSafeStatus = true;
                }
                if (_isOtherGantryChangedToSafeStatus)
                {
                    return _otherTransporterEntity.IsIdle;
                }
                else
                {
                    return false;
                }
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验另外Gantry运动停止情况 
        /// </summary>
        /// <returns></returns>
        private bool CheckOtherAxisPositionStopStatus()
        {
            if (_isOtherStartPosition)
            {
                return _otherTransporterEntity.IsError;
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验对方是否在安全距离
        /// </summary>
        /// <param name="position"></param>
        /// <returns></returns>
        private (bool result,double safeDistance) CheckOtherIsInSafeDistance(double position)
        {
            if (Module.ToString() == ModuleName.Transporter2.ToString())
            {
                if (_runPositive)
                {
                    double safeDistance = _targetPosition + _transporterMinimumDistance;
                    if (position <= safeDistance||(Math.Abs(position - safeDistance) <= 10))
                    {
                        return (false,safeDistance);
                    }
                }
                return (true,0);
            }
            else
            {
                if(!_runPositive)
                {
                    double safeDistance = _targetPosition - _transporterMinimumDistance;
                    if (position >= safeDistance)
                    {
                        return (false,safeDistance);
                    }
                }
                return (true,0);
            }
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            if(Module=="Transporter2")
            {
                _otherModule = "Transporter1";
            }
            else
            {
                _otherModule = "Transporter2";
            }
            _targetPosition = (double)objs[0];
            _runPositive = (bool)objs[1];
            _isConflict = false;
            _isOtherGantryChangedToSafeStatus = false;
            _transporterMinimumDistance = SC.GetValue<int>("Transporter.TransporterMinimumDistance");
            _otherGantryAxis = DEVICE.GetDevice<JetAxisBase>($"{_otherModule}.Gantry");
            _otherTransporterEntity = Singleton<RouteManager>.Instance.GetModule<TransporterEntity>(_otherModule);
            Runner.Start(Module, "Transporter Safe Distance");
            return RState.Running;
        }
    }
}