CyberX8/CyberX8_RT/Devices/AXIS/GalilLipsel/GalilLipselHomeRoutine.cs

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

namespace CyberX8_RT.Devices.AXIS.GalilLipsel
{
   
    public class GalilLipselHomeRoutine : RoutineBase, IRoutine
    {
        private enum HomeStep
        {
            HomingAcceleration,
            HomingDeceleration,
            HomingSpeed,
            ReversePrPosition,
            StartMotion,
            CheckMotion,
            StopMotion,
            CheckStopMotion,
            ForwardMotion,
            StartForwardMotion,
            CheckForwardMotion,
            DPDelay,
            DP,
            WaitDP,
            End
        }

        #region 常量
        private const int MOTION_STOP_CODE = 1;
        private const int ST_STOP_CODE = 4;
        private const int PR_POSITION = -410000;
        private const int RIGHT_PR_POSITION = 50000;
        #endregion

        #region 内部变量
        private JetAxisBase _axis;
        private int _timeout = 5000;
        private double _motionPosition = 0;
        private GalilLipselStopPositionRoutine _stopRoutine;
        private DateTime _stayTime=DateTime.Now;
        private bool _isStay = false;
        private int _homingSpeed = 0;
        private int _homingAcceleration = 0;
        private int _homingDeceleration = 0;
        private int _homingOffset = 0;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        /// <param name="axis"></param>
        public GalilLipselHomeRoutine(string module,JetAxisBase axis) : base(module)
        {
            _axis = axis;
            _stopRoutine=new GalilLipselStopPositionRoutine(module, axis);
        }

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

        public RState Monitor()
        {
            Runner.Run(HomeStep.HomingAcceleration, () => { return _axis.WriteAcceleration(_homingAcceleration); }, _delay_1ms)
                .Run(HomeStep.HomingDeceleration, () => { return _axis.WriteDeceleration(_homingDeceleration); }, _delay_1ms)
                .Run(HomeStep.HomingSpeed, () => { return _axis.WriteSpeed(_homingSpeed); }, _delay_1ms)
                .Run(HomeStep.ReversePrPosition, () => { return _axis.WriteReferencePosition(PR_POSITION); }, _delay_1ms)
                .Run(HomeStep.StartMotion, () => { return _axis.WriteStartMotion(); }, _delay_1ms)
                .WaitWithStopCondition(HomeStep.CheckMotion, CheckMotion,CheckErrorOrWarning,_timeout)
                .Run(HomeStep.StopMotion, () => { return _stopRoutine.Start() == RState.Running; }, _delay_1ms)
                .WaitWithStopCondition(HomeStep.CheckStopMotion, () => { return CommonFunction.CheckRoutineEndState(_stopRoutine); },
                    () => { return CommonFunction.CheckRoutineStopState(_stopRoutine); },_delay_5s)
                .Run(HomeStep.ForwardMotion, () => { return _axis.WriteReferencePosition(RIGHT_PR_POSITION); }, _delay_1ms)
                .Run(HomeStep.StartForwardMotion, () => { return _axis.WriteStartMotion(); }, _delay_1ms)
                .WaitWithStopCondition(HomeStep.CheckForwardMotion, CheckRightMotionEnd, CheckErrorOrWarning, _timeout)
                .Delay(HomeStep.DPDelay, 1000)
                .Run(HomeStep.DP, () => { return _axis.WriteDP(_homingOffset); }, _delay_1ms)
                .Wait(HomeStep.WaitDP, () => { return Math.Round(Math.Abs(_axis.MotionData.MotorPosition - _homingOffset / _axis.ScaleFactor), 2) <= _axis.ToleranceDefault; }, _delay_1s)
                .End(HomeStep.End,NullFun,100);
            return Runner.Status;
        }
        
        /// <summary>
        /// 检验运动（一直运动至motionposition不再变小为止）
        /// </summary>
        /// <returns></returns>
        private bool CheckMotion()
        {
            if (_motionPosition == 0)
            {
                _motionPosition = _axis.MotionData.MotorPosition;
                return false;
            }
            if(_axis.MotionData.MotorPosition<_motionPosition)
            {
                _motionPosition = _axis.MotionData.MotorPosition;
                _isStay = false;
                return false;
            }
            else if (_axis.MotionData.MotorPosition == _motionPosition)
            {
                if (!_isStay)
                {
                    _isStay = true;
                    _stayTime = DateTime.Now;
                }
                else
                {
                    if (DateTime.Now.Subtract(_stayTime).TotalMilliseconds >= 1000)
                    {
                        return true;
                    }
                }
                return false;
            }
            return true;
        }
        /// <summary>
        /// 检验是否运动至右边位置结束
        /// </summary>
        /// <returns></returns>
        private bool CheckRightMotionEnd()
        {
            return _axis.MotionData.StopCode == MOTION_STOP_CODE && !_axis.IsRun;
        }
        /// <summary>
        /// 检验是否出错或告警
        /// </summary>
        /// <returns></returns>
        private bool CheckErrorOrWarning()
        {
            //byte stopCode = _axis.MotionData.StopCode;
            //if (stopCode != 0 && stopCode != MOTION_STOP_CODE)
            //{
            //    LOG.WriteLog(eEvent.ERR_AXIS, Module, $"axis home stopcode is {stopCode}");
            //    return true;
            //}
            return false;
        }
        public RState Start(params object[] objs)
        {
            _timeout = (int)objs[0];
            _homingAcceleration=(int)objs[1];
            _homingDeceleration= (int)objs[2];
            _homingSpeed=(int)objs[3];
            _homingOffset = (int)objs[4];
            _motionPosition = 0;
            _isStay = false;
            return Runner.Start(Module, "Home");
        }
    }
}