CyberX8/CyberX8_RT/Devices/AXIS/Yaskawa/YaskawaProfilePositionRoutine.cs

using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.CommonData.PUF;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.TwinCat;
using CyberX8_Core;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CyberX8_RT.Devices.AXIS.Yaskawa
{
   
    public class YaskawaProfilePositionRoutine : RoutineBase, IRoutine
    {
        private enum ProfilePositionStep
        {
            WriteInitControlWord,
            ProfileModeOfOperation,
            WriteProfileParameter,
            WriteProfilePositionControlWord,
            Delay,
            CheckRun,
            NoneModeOfOperation,
            EnableOperation,
            End
        }
        #region 常量 
        private const string PROFILE_VELOCITY = "ProfileVelocity";
        private const string PROFILE_ACCEL = "ProfileAccel";
        private const string PROFILE_DECEL = "ProfileDecel";
        private const string TARGET_POSITION = "TargetPosition";
        #endregion
        #region 内部变量
        private JetAxisBase _axis;
        private int _profileVelocity = 0;
        private int _profileAcceleration = 0;
        private int _profileDeceleration = 0;
        private int _targetPosition;
        private bool _judgeTorqueLimit = false;
        #endregion
        public YaskawaProfilePositionRoutine(string module,JetAxisBase axis) : base(module)
        {
            _axis = axis;
        }

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

        public RState Monitor()
        {
            Runner.Run(ProfilePositionStep.WriteInitControlWord, () => { return _axis.WriteControlWord(0x0F); },() => { return CheckControlWord(0x0F); },500)
                .Run(ProfilePositionStep.ProfileModeOfOperation, () => { return _axis.WriteModeOfMode(AxisModeOfOperation.ProfilePositionMode); },()=> { return CheckModeOfOperation((byte)AxisModeOfOperation.ProfilePositionMode); }, 1000)
                .Run(ProfilePositionStep.WriteProfileParameter, () => { return WriteProfileParameter(); },NullFun,100)
                .Run(ProfilePositionStep.WriteProfilePositionControlWord, () => { return _axis.WriteControlWord(0x3F); }, () => { return CheckControlWord(0x3F); },1000)
                .Delay(ProfilePositionStep.Delay,1000)
                .WaitWithStopCondition(ProfilePositionStep.CheckRun,CheckRun,CheckRunStop)
                .Run(ProfilePositionStep.NoneModeOfOperation, () => { return _axis.WriteModeOfMode(AxisModeOfOperation.None); },() => { return CheckModeOfOperation((byte)AxisModeOfOperation.None); },1000)
                .Run(ProfilePositionStep.EnableOperation, () => { return _axis.EnableOperation(); }, () => { return CheckControlWord(0x0F); },1000)
                .End(ProfilePositionStep.End,NullFun,100);
            return Runner.Status;
        }

        private bool WriteProfileParameter()
        {
            bool result= BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{PROFILE_VELOCITY}", _profileVelocity);
            if(!result)
            {
                return false;
            }
            result=BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{PROFILE_ACCEL}", _profileAcceleration);
            if(!result)
            {
                return false;
            }
            result=BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{PROFILE_DECEL}", _profileDeceleration);
            if(!result)
            {
                return false;
            }
            result= BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{TARGET_POSITION}", _targetPosition);
            return result;
        }

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

        public bool CheckControlWord(ushort controlWord)
        {
            return _axis.ControlWord == controlWord;
        }
        /// <summary>
        /// 检验是否正常运行
        /// </summary>
        /// <returns></returns>
        private bool CheckRun()
        {
            if (_judgeTorqueLimit)
            {
                return _axis.InTargetPosition && !_axis.IsRun && _axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
            }
            else
            {
                bool isTorqueOver = TorqueIsOverLimit();
                //过载同时掉电
                if (isTorqueOver && !_axis.IsSwitchOn)
                {
                    return true;
                }
                if(_axis.InTargetPosition&&_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition))
                {
                    return true;
                }                
                return false;
            }
        }
        /// <summary>
        /// Torque是否过载
        /// </summary>
        /// <returns></returns>
        private bool TorqueIsOverLimit()
        {
            return !_axis.MotionData.TorqueLimited;
        }
        /// <summary>
        /// 检验是否停止 
        /// </summary>
        /// <returns></returns>
        private bool CheckRunStop()
        {
            //没有运动
            bool isStop = !_axis.IsRun&&!_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
            if (isStop&&_judgeTorqueLimit)
            {
                ErrorMsg = "position is stop";
                return true;
            }
            if (_judgeTorqueLimit)
            {
                if (TorqueIsOverLimit())
                {
                    ErrorMsg = "Torque is over limit";
                    return true;
                }
            }
            return false;
        }
        public RState Start(params object[] objs)
        {
            _targetPosition = (int)objs[0];
            _profileVelocity = (int)objs[1];
            _profileAcceleration = (int)objs[2];
            _profileDeceleration = (int)objs[3];
            if(objs.Length>=5)
            {
                _judgeTorqueLimit = (bool)objs[4];
            }
            return Runner.Start(Module, "ProfilePosition");
        }
    }
}