PunkHPX8/PunkHPX8_RT/Devices/AXIS/Kollmorgen/KollmorgenHomeRoutine.cs

using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.Routine;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using PunkHPX8_Core;
using PunkHPX8_RT.Devices.AXIS;

namespace CyberX12_RT.Devices.AXIS.Kollmorgen
{
    public class KollmorgenHomeRoutine : RoutineBase, IRoutine
    {
        private enum HomeStep
        {
            WriteOffline,
            WriteOnline,
            WriteControlWord,
            HomeModeOfOperation,
            WriteHomeControlWord,
            HomeDelay,
            CheckHome,
            LastDelay,
            NoneModeOfOperation,
            EnableOperation,
            WriteTargetPosition,
            End
        }

        #region 常量
        private const string TARGET_POSITION = "TargetPosition";
        #endregion

        #region 内部变量
        private JetAxisBase _axis;
        private BeckhoffCommonAxis _beckhoffCommonAxis;
        private int _timeout = 5000;
        private byte _homingMethod = 0;
        private bool _isLogError = true;
        #endregion

        public KollmorgenHomeRoutine(string module, JetAxisBase axis, BeckhoffCommonAxis beckhofCommonAxis) : base(module)
        {
            _axis = axis;
            _beckhoffCommonAxis = beckhofCommonAxis;
        }


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

        public RState Monitor()
        {
            Runner
                .Run(HomeStep.WriteControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); }, 1000)
                .Run(HomeStep.HomeModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.HomingMode); },
                () => { return CheckModeOfOperation((byte)AxisModeOfOperation.HomingMode); }, 1000)
                .Run(HomeStep.WriteHomeControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x1F);}, () => { return CheckControlWord(0x1F); }, 1000)
                .Delay(HomeStep.HomeDelay, 100)
                .WaitWithStopCondition(HomeStep.CheckHome, CheckHome, CheckErrorOrWarning, _timeout, _isLogError)
                .Delay(HomeStep.LastDelay, 1000)
                .Run(HomeStep.NoneModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.None); },
                () => { return CheckModeOfOperation((byte)AxisModeOfOperation.None); }, 1000)
                .Run(HomeStep.EnableOperation, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); }, 1000)
                .Run(HomeStep.WriteTargetPosition,WriteTargetPosition,_delay_1ms)
                .End(HomeStep.End, NullFun, 100);
            return Runner.Status;
        }

        public bool CheckModeOfOperation(byte modeOfOperation)
        {
            return _axis.ModeOfOperation == modeOfOperation;
        }

        public bool CheckControlWord(ushort controlWord)
        {
            return _axis.ControlWord == controlWord;
        }
        /// <summary>
        /// 设置目标位置为当前位置
        /// </summary>
        /// <param name="targetPosition"></param>
        private bool WriteTargetPosition()
        {
            return _beckhoffCommonAxis.WriteVariable(TARGET_POSITION, 0);
        }
        private bool CheckHome()
        {
            LOG.WriteLog(eEvent.INFO_AXIS, Module, $"StatusWord is {_axis.MotionData.StatusWord}");
            _axis.UpdateStatusWord(_axis.MotionData.StatusWord);
            return _axis.IsHomed;
        }
        /// 检验是否出错或告警
        /// </summary>
        /// <returns></returns>
        private bool CheckErrorOrWarning()
        {
            bool result = _axis.MotionData.Status.ToLower().Contains("error") || _axis.MotionData.Status.ToLower().Contains("warning");
            if (result)
            {
                LOG.WriteLog(eEvent.ERR_AXIS, Module, $"axis home occur error or warning");
            }
            return result;
        }
        public RState Start(params object[] objs)
        {
            _timeout = (int)objs[0];
            _homingMethod = (byte)objs[1];
            if (objs.Length >= 3)
            {
                _isLogError = (bool)objs[2];
            }
            return Runner.Start(Module, "Home");
        }
    }
}