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.Equipment;
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.PUF;
using CyberX8_RT.Devices.SRD;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using CyberX8_RT.Devices.Safety;

namespace CyberX8_RT.Modules.Transporter
{
    public class SRDHomeRoutine : RoutineBase, IRoutine
    {
        private enum HomeAllStep
        {
            CheckSafety,
            CheckPreCondition,
            CloseDoor,
            CheckDoorClosed,
            HomeSRDArm,
            CheckArmHome,
            ArmHome,
            ArmHomeWait,
            HomeSRDRotation,
            RotationHome,
            RotationHomeWait,
            CheckRotationHome,
            OpenDoor,
            CheckDoorOpened,
            End
        }
        #region 内部变量
        private JetAxisBase _armAxis;
        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.HomeSRDArm, ArmAxisHome, _delay_1ms)
                .WaitWithStopCondition(HomeAllStep.CheckArmHome, () => { return _armAxis.IsHomed && _armAxis.Status == RState.End; }, () => { return _armAxis.Status == RState.Failed; })               
                .Run(HomeAllStep.HomeSRDRotation, RotationAxisHome,_delay_1ms)
                .WaitWithStopCondition(HomeAllStep.CheckRotationHome, () => { return _rotationAxis.IsHomed && _rotationAxis.Status == RState.End; }, () => { return _rotationAxis.Status == RState.Failed; })
                .Run(HomeAllStep.ArmHome, () => { return _armAxis.PositionStation("Home", true); }, NullFun, 100)
                .WaitWithStopCondition(HomeAllStep.ArmHomeWait, () => { return _armAxis.Status == RState.End; }, () => { return _armAxis.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.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.TwincatState != 8)
            {
                LOG.WriteLog(eEvent.ERR_SRD, Module.ToString(), "Twincat Status is not OP status");
                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(! _armAxis.IsSwitchOn)
            {
                LOG.WriteLog(eEvent.ERR_SRD, Module, $"{_module}.Arm is switchoff");
                return false;
            }
            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>
        /// Arm Home
        /// </summary>
        /// <returns></returns>
        private bool ArmAxisHome()
        {
            return _armAxis.Home();
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _armAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Arm");
            _rotationAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Rotation");
            _common = DEVICE.GetDevice<SrdCommonDevice>($"{Module}.Common");
            _doorCloseRoutine = new SrdCommonDoorCloseRoutine(Module);
            return Runner.Start(Module, "Home");
        }
    }
}