PunkHPX8/PunkHPX8_RT/Modules/SRD/SRDRotationRoutine.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Fsm;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.Beckhoff.AxisProvider;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.RecipeCenter;
using MECF.Framework.Common.Utilities;
using PunkHPX8_Core;
using PunkHPX8_RT.Devices.AXIS;
using PunkHPX8_RT.Devices.SRD;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;

namespace PunkHPX8_RT.Modules.SRD
{
    internal class SRDRotationRoutine : ModuleRoutineBase, IRoutine
    {
        private enum RotationStep
        {
            Rotation,
            Wait,
            End
        }
        #region 常量
        /// <summary>
        /// ROTATION电机转速比例
        /// </summary>
        private const int SPEED_RATIO = 1;
        #endregion

        #region 内部变量
        /// <summary>
        /// 模块名称
        /// </summary>
        private string _module;
        /// <summary>
        /// Rotation Axis
        /// </summary>
        private JetAxisBase _rotationAxis;
        /// <summary>
        /// 转速
        /// </summary>
        private double _speed;
        /// <summary>
        /// 时间
        /// </summary>
        private double _time;
        /// <summary>
        /// 超时时间
        /// </summary>
        private int _timeOut = 6000;
        /// <summary>
        /// SRD rotation Provider对象
        /// </summary>
        private BeckhoffProviderAxis _rotationProviderAxis;
        /// <summary>
        /// SRD Common
        /// </summary>
        private SrdCommonDevice _srdCommon;
        #endregion

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

        public SRDRotationRoutine(ModuleName module, JetAxisBase rotationAxis) : base(module)
        {
            Name = "StartRotation";
            _module = module.ToString();
            _srdCommon = DEVICE.GetDevice<SrdCommonDevice>($"{module}.Common");
        }

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

        public RState Monitor()
        {
            Runner.Run(RotationStep.Rotation, StartRotation, NullFun, _timeOut)
                .WaitWithStopCondition(RotationStep.Wait, CheckRotationStatus, CheckRotationRunStop)
                .End(RotationStep.End, NullFun);
            return Runner.Status;
        }

        /// <summary>
        /// 开始旋转
        /// </summary>
        /// <returns></returns>
        public bool StartRotation()
        {           

            if (!_rotationAxis.IsSwitchOn)
            {
                LOG.WriteLog(eEvent.ERR_SRD, _module, $"{_module}.Rotation is switchoff");
                return false;
            }
            if (!_rotationAxis.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_SRD, _module, $"{_module}.Rotation is not homed");
                return false;
            }
            _rotationProviderAxis = BeckhoffAxisProviderManager.Instance.GetAxisProvider($"{_module}.Rotation");
            if (_rotationProviderAxis == null)
            {
                LOG.WriteLog(eEvent.ERR_SRD, _module, $"{_module}.Rotation Provider is not exist");
                return false;
            }
            if (!_srdCommon.RotationInterLock())
            {
                return false;
            }
            if (_srdCommon.CommonData.LiftUp)
            {
                LOG.WriteLog(eEvent.ERR_SRD, _module, $"{_module} LiftUp is on");
                return false;
            }
            if (_srdCommon.CommonData.LiftUpStatus)
            {
                LOG.WriteLog(eEvent.ERR_SRD, _module, $"{_module} LiftUp sensor is on");
                return false;
            }
            double _scale = _rotationProviderAxis.ScaleFactor;
            //获取当前Position
            double currentPosition = _rotationAxis.MotionData.MotorPosition;
            //转成角速度（deg/s）
            double DPSspeed = BeckhoffVelocityUtil.ConvertVelocityToDegPerSecondByRPM((int)_speed);
            int rotationSpeed = (int)Math.Round(_scale * DPSspeed, 0);
            int targetPosition = (int)Math.Round(_scale * (_time * DPSspeed + currentPosition), 0);
            LOG.WriteLog(eEvent.INFO_SRD, _module, "Start Rotation");
            return _rotationAxis.ProfilePosition(targetPosition, rotationSpeed * SPEED_RATIO, 0, 0);
                       
        }
        /// <summary>
        /// 检验移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationStatus()
        {
            return _rotationAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationRunStop()
        {
            return _rotationAxis.Status == RState.Failed;
        }

        public RState Start(params object[] objs)
        {
            _rotationAxis = (JetAxisBase)objs[0];
            _time = (double)objs[1];
            _speed = (double)objs[2];

            Runner.Start(Module, Name);
            return RState.Running;
        }
    }
}