PunkHPX8/PunkHPX8_RT/Devices/AXIS/Galil/GalilHomeRoutine.cs

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

namespace PunkHPX8_RT.Devices.AXIS.Galil
{
   
    public class GalilHomeRoutine : RoutineBase, IRoutine
    {
        private enum HomeStep
        {
            HomingAcceleration,
            HomingDeceleration,
            HomingSpeed,
            WriteCNType,
            SetHomeModel,
            StartMotion,
            Delay,
            CheckHomingEnd,
            DPDelay,
            DP,
            WaitDP,
            End
        }

        #region 常量
        private const int HOME_STOP_CODE = 10;
        #endregion

        #region 内部变量
        private JetAxisBase _axis;
        private GalilCommonAxis _galilCommonAxis;
        private int _timeout = 5000;
        private int _homingAcceleration = 0;
        private int _homingDeceleration = 0;
        private int _homingSpeed = 0;
        private int _homingOffset = 0;
        private int _cnType = 0;
        #endregion

        public GalilHomeRoutine(string module,JetAxisBase axis,GalilCommonAxis galilCommonAxis) : base(module)
        {
            _axis = axis;
            _galilCommonAxis = galilCommonAxis;
        }

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

        public RState Monitor()
        {
            Runner.Run(HomeStep.HomingAcceleration, () => { return _galilCommonAxis.WriteAcceleration(_homingAcceleration); }, _delay_1ms)
                .Run(HomeStep.HomingDeceleration, () => { return _galilCommonAxis.WriteDeceleration(_homingDeceleration); }, _delay_1ms)
                .Run(HomeStep.HomingSpeed, () => { return _galilCommonAxis.WriteSpeed(_homingSpeed); }, _delay_1ms)
                //增加CN类型用于区分process transporter gantry与elevator home方向相反
                .RunIf(HomeStep.WriteCNType, _cnType!=0, () => { return _galilCommonAxis.WriteCNCommand($",{_cnType}"); },_delay_1ms)
                .Run(HomeStep.SetHomeModel, () => { return _galilCommonAxis.WriteHomeAxisCommand(); }, _delay_1ms)
                .Run(HomeStep.StartMotion, () => { return _axis.WriteStartMotion(); }, _delay_1ms)
                .Delay(HomeStep.Delay,500)
                .WaitWithStopCondition(HomeStep.CheckHomingEnd, () => { 
                    return _axis.MotionData.StopCode == HOME_STOP_CODE&&!_axis.IsRun; },CheckErrorOrWarning,_timeout)
                .Delay(HomeStep.DPDelay,1000)
                .Run(HomeStep.DP, () => { return _galilCommonAxis.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>
        /// 检验是否出错或告警
        /// </summary>
        /// <returns></returns>
        private bool CheckErrorOrWarning()
        {
            //byte stopCode = _axis.MotionData.StopCode;
            //if (stopCode != 0 && stopCode != HOME_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];
            _cnType = objs.Length >= 6 ? (int)objs[5] : 0;
            return Runner.Start(Module, "Home");
        }
    }
}