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

using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using PunkHPX8_Core;
using MECF.Framework.Common.Beckhoff.AxisProvider;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.Routine;
using System;

namespace PunkHPX8_RT.Devices.AXIS.Kollmorgen
{
    public class KollmorgenStopPositionRoutine : RoutineBase, IRoutine
    {
        private enum StopPositionStep
        {
            SetHaltControlWord,
            CheckStop,
            Delay,
            WriteTargePosition,
            NoneModeOfOperation,
            QuickStop,
            ControlOffControlWord,
            ControlOnControlWord,
            EnableOperation,
            End
        }
        #region 常量
        private const string TARGET_POSITION = "TargetPosition";
        #endregion

        #region 内部变量
        private JetAxisBase _axis;
        private int _timeout = 5000;
        private int _targetPosition;
        /// <summary>
        /// SRD rotation Provider对象
        /// </summary>
        private BeckhoffProviderAxis _rotationProviderAxis;
        private BeckhoffCommonAxis _beckhoffCommonAxis;
        #endregion

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

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

        public RState Monitor()
        {
            Runner.Run(StopPositionStep.SetHaltControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0X10F); }, () => { return CheckControlWord(0X10F); }, 1000)
                .WaitWithStopCondition(StopPositionStep.CheckStop, CheckStop, CheckErrorOrWarning, _timeout)
                .Delay(StopPositionStep.Delay,_delay_1s)
                .Run(StopPositionStep.WriteTargePosition, () => { return WriteTargetPosition(); }, NullFun, 100)
                .Run(StopPositionStep.NoneModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.None); }, () => { return CheckModeOfOperation((byte)AxisModeOfOperation.None); }, 1000)
                .Run(StopPositionStep.QuickStop, () => { return _beckhoffCommonAxis.WriteControlWord(0x02); }, () => { return CheckControlWord(0x02); }, 1000)
                .Run(StopPositionStep.ControlOffControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x06); }, () => { return CheckControlWord(0x06); }, 1000)
                .Run(StopPositionStep.ControlOnControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x07); }, () => { return CheckControlWord(0x07); }, 1000)
                .Run(StopPositionStep.EnableOperation, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); }, 1000)
                .End(StopPositionStep.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()
        {
            double scale = _rotationProviderAxis.ScaleFactor;
            _targetPosition = (int)Math.Ceiling(_axis.MotionData.MotorPosition * scale);
            return _beckhoffCommonAxis.WriteVariable(TARGET_POSITION, _targetPosition);
        }
        /// <summary>
        /// 检查是否运动完成
        /// </summary>
        /// <returns></returns>
        private bool CheckRunEnd()
        {
            bool result = _axis.InTargetPosition && !_axis.IsRun;
            return result;
        }
        /// <summary>
        /// 检查是否运动完成
        /// </summary>
        /// <returns></returns>
        private bool CheckRunStop()
        {
            //没有到达目标同时没有运动
            bool isStop = !_axis.IsRun && !_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
            if (isStop)
            {
                ErrorMsg = "position is stop";
                return true;
            }
            return false;
        }
        /// <summary>
        /// 检查Halt是否停下来
        /// </summary>
        /// <returns></returns>
        private bool CheckStop()
        {
            return !_axis.IsRun;
        }

        /// 检验是否出错或告警
        /// </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 stop occur error or warning");
            }
            return result;
        }
        public RState Start(params object[] objs)
        {
            _rotationProviderAxis = BeckhoffAxisProviderManager.Instance.GetAxisProvider($"{Module}");
            if (_rotationProviderAxis == null)
            {
                LOG.WriteLog(eEvent.ERR_AXIS, Module, $"{Module} Axist Provider is not exist");
                return RState.Failed;
            }
            return Runner.Start(Module, "Stop Position");
        }
    }
}