PunkHPX8/PunkHPX8_RT/Devices/AXIS/CanOpen/CanOpenProfilePositionRoutine.cs

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

namespace PunkHPX8_RT.Devices.AXIS.CANOpen
{
   
    public class CanOpenProfilePositionRoutine : 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;
        private BeckhoffCommonAxis _beckhoffCommonAxis;
        #endregion
        public CanOpenProfilePositionRoutine(string module,JetAxisBase axis,BeckhoffCommonAxis beckhoffCommonAxis) : base(module)
        {
            _axis = axis;
            _beckhoffCommonAxis = beckhoffCommonAxis;
        }

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

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

        private bool WriteProfileParameter()
        {
            bool result= _beckhoffCommonAxis.WriteVariable(PROFILE_VELOCITY, _profileVelocity);
            if(!result)
            {
                return false;
            }
            result=_beckhoffCommonAxis.WriteVariable(PROFILE_ACCEL, _profileAcceleration);
            if(!result)
            {
                return false;
            }
            result=_beckhoffCommonAxis.WriteVariable(PROFILE_DECEL, _profileDeceleration);
            if(!result)
            {
                return false;
            }
            result= _beckhoffCommonAxis.WriteVariable(TARGET_POSITION, _targetPosition);
            return result;
        }

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

        public bool CheckControlWord(ushort controlWord)
        {
            return _axis.ControlWord == controlWord;
        }

        private bool CheckRun()
        {
            if (_judgeTorqueLimit)
            {
                return _axis.InTargetPosition && !_axis.IsRun && _axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
            }
            else
            {
                bool isTorqueOver = TorqueIsOverLimit();
                //过载同时掉电
                if(isTorqueOver&&!_axis.IsSwitchOn)
                {
                    return true;
                }
                return false;
            }
        }
        /// <summary>
        /// Torque是否过载
        /// </summary>
        /// <returns></returns>
        private bool TorqueIsOverLimit()
        {
            return !_axis.MotionData.TorqueLimited;
        }

        private bool CheckRunStop()
        {
            //没有到达目标同时没有运动
            bool isStop = !_axis.IsRun&&!_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
            if (isStop)
            {
                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");
        }
    }
}