CyberX8/CyberX8_RT/Modules/SRD/SRDHomeRoutine.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.Utilities;
using CyberX8_Core;
using CyberX8_RT.Devices.AXIS;
using CyberX8_RT.Devices.Facilities;
using CyberX8_RT.Devices.SRD;
using CyberX8_RT.Devices.Safety;

namespace CyberX8_RT.Modules.Transporter
{
    public class SRDHomeRoutine : RoutineBase, IRoutine
    {
        private enum HomeAllStep
        {
            //CheckSafety,
            CheckPreCondition,
            CloseDoor,
            CheckDoorClosed,
            HomeSRDRotation,
            CheckRotationHome,
            RotationHome,
            RotationHomeWait,
            LiftUpOn,
            //FlippersOut100,
            FlippersOut150,
            FlippersOut200,
            OpenDoor,
            CheckDoorOpened,
            End
        }
        #region 内部变量
        private JetAxisBase _rotationAxis;
        private SrdCommonDevice _common;
        private SrdCommonDoorCloseRoutine _doorCloseRoutine;
        private string _module;
        #endregion

        #region 属性
        /// <summary>
        /// 当前子状态机
        /// </summary>
        public string CurrentStateMachine
        {
            get { return Runner.CurrentStep.ToString(); }
        }
        #endregion

        public SRDHomeRoutine(string module) : base(module)
        {
            _module = module;
        }

        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }

        public RState Monitor()
        {
            Runner//.Run(HomeAllStep.CheckSafety,CheckSafety,_delay_1ms)
                .Run(HomeAllStep.CheckPreCondition, CheckPreCondition,NullFun,_delay_1ms)
                .Run(HomeAllStep.CloseDoor, () => { return _doorCloseRoutine.Start(true) == RState.Running; }, NullFun, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.CheckDoorClosed, () => CommonFunction.CheckRoutineEndState(_doorCloseRoutine), () => CommonFunction.CheckRoutineStopState(_doorCloseRoutine))                
                .Run(HomeAllStep.HomeSRDRotation, RotationAxisHome,_delay_1ms)
                .WaitWithStopCondition(HomeAllStep.CheckRotationHome, () => { return _rotationAxis.IsHomed && _rotationAxis.Status == RState.End; }, () => { return _rotationAxis.Status == RState.Failed; })               
                .Run(HomeAllStep.RotationHome, () => { return _rotationAxis.PositionStation("Home",true); }, NullFun, 100)
                .WaitWithStopCondition(HomeAllStep.RotationHomeWait, () => { return _rotationAxis.Status == RState.End; }, () => { return _rotationAxis.Status == RState.Failed; })
                .Run(HomeAllStep.LiftUpOn, LiftUpOn, CheckLiftUpOnEndStatus, CheckLiftUpOnStopStatus)
                //.Run(HomeAllStep.FlippersOut100, () => { return FlippersOut(100); }, CheckFlippersOutEndStatus, CheckFlippersOutStopStatus)
                .Run(HomeAllStep.FlippersOut150, () => { return FlippersOut(150); }, CheckFlippersOutEndStatus, CheckFlippersOutStopStatus)
                .Run(HomeAllStep.FlippersOut200, () => { return FlippersOut(200); }, CheckFlippersOutEndStatus, CheckFlippersOutStopStatus)
                .Run(HomeAllStep.OpenDoor, () => { return _doorCloseRoutine.Start(false) == RState.Running; },NullFun,_delay_1ms)                
                .WaitWithStopCondition(HomeAllStep.CheckDoorOpened,()=>CommonFunction.CheckRoutineEndState(_doorCloseRoutine),()=>CommonFunction.CheckRoutineStopState(_doorCloseRoutine))
                .End(HomeAllStep.End,NullFun);

            return Runner.Status; 
        }
        /// <summary>
        /// 检验Safety
        /// </summary>
        /// <returns></returns>
        private bool CheckSafety()
        {
            SafetyDevice safetyDevice = DEVICE.GetDevice<SafetyDevice>("Safety");
            if (safetyDevice == null)
            {
                LOG.WriteLog(eEvent.ERR_SRD, Module.ToString(), "Safety device is null");
                return false;
            }           
            if (safetyDevice.SafetyData.SrdCommErr)
            {
                LOG.WriteLog(eEvent.ERR_SRD, Module.ToString(), "Twincat SRD Communication status is error");
                return false;
            }
            if (safetyDevice.SafetyData.SrdFunctionBlockErr)
            {
                LOG.WriteLog(eEvent.ERR_SRD, Module.ToString(), "Twincat SRD function block status is error");
                return false;
            }
            return true;
        }
        /// <summary>
        /// 检验前置条件
        /// </summary>
        /// <returns></returns>
        private bool CheckPreCondition()
        {
            //检查电机谁否开启           
            if(!_rotationAxis.IsSwitchOn)
            {
                LOG.WriteLog(eEvent.ERR_SRD, Module, $"{_module}.Rotation is switchoff");
                return false;
            }
            //检查CDA与N2是否开启
            SystemFacilities systemFacilities = DEVICE.GetDevice<SystemFacilities>("System.Facilities");
            var result = systemFacilities.CheckCDAN2();
            if (!result.result)
            {
                LOG.WriteLog(eEvent.ERR_SRD, Module, $"CDA or N2 is not enabled");
                return false;
            }
            return true;
        }
        /// <summary>
        /// Rotation Home
        /// </summary>
        /// <returns></returns>
        private bool RotationAxisHome()
        {
            return _rotationAxis.Home();
        }
        /// <summary>
        /// 检验Rotation Home状态
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationHome()
        {
            return _rotationAxis.IsHomed&&_rotationAxis.Status==RState.End;
        }
        /// <summary>
        /// LiftUpOn
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool LiftUpOn()
        {
            bool result = _common.LiftUpOnAction("", null);
            if (!result)
            {
                NotifyError(eEvent.ERR_SRD, "Lift Up On Action is failed", 0);
                return result;
            }
            return true;
        }
        /// <summary>
        /// 检验LiftUpOnEnd状态
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool CheckLiftUpOnEndStatus()
        {
            return _common.Status == RState.End;
        }
        /// <summary>
        /// 检验LiftUpOnStop状态
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool CheckLiftUpOnStopStatus()
        {
            if (_common.Status == RState.Failed || _common.Status == RState.Timeout)
            {
                NotifyError(eEvent.ERR_SRD, "Check LiftUpOn is failed", 0);
                return true;
            }
            return false;
        }
        /// <summary>
        /// Flippers Out
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool FlippersOut(int size)
        {
            bool result = false;
            object[] objects = new object[1];
            objects[0] = size;
            result = _common.FlipperOutAction("", objects);
            if (!result)
            {
                NotifyError(eEvent.ERR_SRD, $"FlipperOut Action is failed", 0);
                return result;
            }

            return true;
        }
        /// <summary>
        /// 检验FlippersOut结束状态
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool CheckFlippersOutEndStatus()
        {
            return _common.Status == RState.End;
        }
        /// <summary>
        /// 检验FlippersOut结束状态
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        private bool CheckFlippersOutStopStatus()
        {
            if (_common.Status == RState.Failed || _common.Status == RState.Timeout)
            {
                NotifyError(eEvent.ERR_SRD, $"Check FlipperOut Action is failed", 0);
                return true;
            }
            return false;
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _rotationAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Rotation");
            _common = DEVICE.GetDevice<SrdCommonDevice>($"{Module}.Common");
            _doorCloseRoutine = new SrdCommonDoorCloseRoutine(Module);
            return Runner.Start(Module, "Home");
        }
    }
}