PunkHPX8/PunkHPX8_RT/Devices/AXIS/Galil/GalilProfilePositionRoutine.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 PunkHPX8_Core;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Diagnostics;

namespace PunkHPX8_RT.Devices.AXIS.Galil
{
   
    public class GalilProfilePositionRoutine : RoutineBase, IRoutine
    {
        private enum ProfilePositionStep
        {
            WriteAcceleration,
            WriteDeceleration,
            WriteSpeed,
            WriteTargetPosition,
            StartMotion,
            Delay,
            CheckRunEnd,
            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";
        private const int MOTION_STOP_CODE = 1;
        #endregion
        #region 内部变量
        private JetAxisBase _axis;
        private GalilCommonAxis _galilCommonAxis;
        private int _profileVelocity = 0;
        private int _profileAcceleration = 0;
        private int _profileDeceleration = 0;
        private int _targetPosition;
        private bool _judgeTorqueLimit = false;
        private Stopwatch _stopWatch = new Stopwatch();
        #endregion
        public GalilProfilePositionRoutine(string module,JetAxisBase axis,GalilCommonAxis galilCommonAxis) : base(module)
        {
            _axis = axis;
            _galilCommonAxis = galilCommonAxis;
        }

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

        public RState Monitor()
        {
            Runner.Run(ProfilePositionStep.WriteAcceleration, () => { return _galilCommonAxis.WriteAcceleration(_profileAcceleration); },_delay_1ms)
                .Run(ProfilePositionStep.WriteDeceleration, () => { return _galilCommonAxis.WriteDeceleration(_profileDeceleration); }, _delay_1ms)
                .Run(ProfilePositionStep.WriteSpeed, () => { return _galilCommonAxis.WriteSpeed(_profileVelocity); }, _delay_1ms)
                .Run(ProfilePositionStep.WriteTargetPosition, () => { return _galilCommonAxis.WriteAbsolutePosition(_targetPosition); }, _delay_1ms)
                .Run(ProfilePositionStep.StartMotion, () =>_axis.WriteStartMotion(), _delay_1ms)
                .Delay(ProfilePositionStep.Delay,500)
                .WaitWithStopCondition(ProfilePositionStep.CheckRunEnd,CheckRun,CheckRunStop)
                .End(ProfilePositionStep.End,NullFun,100);
            return Runner.Status;
        }
        /// <summary>
        /// 检验是否正常运行
        /// </summary>
        /// <returns></returns>
        private bool CheckRun()
        {
            bool isInTargetPosition = _axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
            if (_judgeTorqueLimit)
            {
                if(!_axis.IsRun&& isInTargetPosition)
                {
                    return true;
                }
                return false;
            }
            else
            {
                bool isTorqueOver = TorqueIsOverLimit();
                //过载同时掉电
                if (isTorqueOver && !_axis.IsSwitchOn)
                {
                    return true;
                }
                if(!_axis.IsRun&&isInTargetPosition)
                {
                    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)
        {
            _stopWatch.Reset();
            _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");
        }
    }
}