CyberX8/CyberX8_RT/Modules/PUF/PufReadyForRobotPickRoutine.cs

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

namespace CyberX8_RT.Modules.PUF
{
    public class PufReadyForRobotPickRoutine : RoutineBase, IRoutine
    {
        private enum ReadyForPickPlaceStep
        {
            RotationRobot,
            WaitRotationRobot,
            VerticalRobot,
            WaitVerticalRobot,
            //ChuckAVacuumOff,
            End
        }
        #region 内部变量
        private JetAxisBase _flipAxis;
        private JetAxisBase _rotationAxis;
        private JetAxisBase _verticalAxis;
        private PufVacuum _vacuum;
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public PufReadyForRobotPickRoutine(string module) : base(module)
        {
        }

        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("manual abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(ReadyForPickPlaceStep.RotationRobot, () => AxisGotoPosition(_rotationAxis,"Robot",0), _delay_1ms)
                .WaitWithStopCondition(ReadyForPickPlaceStep.WaitRotationRobot, CheckRotationPositionStatus, ()=>CheckRotationStopStatus(0))
                .Run(ReadyForPickPlaceStep.VerticalRobot, () => _verticalAxis.PositionStation("Robot"), _delay_1ms)
                .WaitWithStopCondition(ReadyForPickPlaceStep.WaitVerticalRobot, CheckVerticalPositionStatus, ()=>CheckVerticalStopStatus(0))
                //.Run(ReadyForPickPlaceStep.ChuckAVacuumOff,SideAVacuumOff, _delay_1ms)
                .End(ReadyForPickPlaceStep.End, NullFun, _delay_1ms);
            return Runner.Status;
        }

        /// <summary>
        /// Axis goto position
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="position"></param>
        /// <param name="index"></param>
        /// <returns></returns>
        private bool AxisGotoPosition(JetAxisBase axis, string position, int index)
        {
            bool result = axis.PositionStation(position);
            if (!result)
            {
                NotifyError(eEvent.ERR_PUF, $"{axis.Module} goto {position} failed", index);
            }
            return result;
        }
        /// <summary>
        /// 检验Rotation移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationPositionStatus()
        {
            return _rotationAxis.Status == RState.End;
        }

        /// <summary>
        /// 检验Rotation异常状态
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationStopStatus(int index)
        {
            bool result = _rotationAxis.Status == RState.Failed || _rotationAxis.Status == RState.Timeout;
            if (result)
            {
                NotifyError(eEvent.ERR_PUF, "rotation motion failed", index);
            }
            return result;
        }

        /// <summary>
        /// 检验Vertical移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckVerticalPositionStatus()
        {
            return _verticalAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Vertical异常状态
        /// </summary>
        /// <returns></returns>
        private bool CheckVerticalStopStatus(int index)
        {
            bool result = _verticalAxis.Status == RState.Failed || _verticalAxis.Status == RState.Timeout;
            if (result)
            {
                NotifyError(eEvent.ERR_PUF, "vetical motion failed", index);
            }
            return result;
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            InitializeParameters();
            return Runner.Start(Module, "start ready for robot place");
        }
        /// <summary>
        /// 初始化参数
        /// </summary>
        private void InitializeParameters()
        {
            _flipAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Flip");
            _rotationAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Rotation");
            _verticalAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Vertical");
            _vacuum = DEVICE.GetDevice<PufVacuum>($"{Module}.Vacuum");
        }
        /// <summary>
        /// 重试
        /// </summary>
        /// <param name="step"></param>
        public RState Retry(int step)
        {
            InitializeParameters();
            List<Enum> preStepIds = new List<Enum>();
            return Runner.Retry(ReadyForPickPlaceStep.RotationRobot, preStepIds, Module, "ReadyForPick Retry");
        }
        /// <summary>
        /// 检验完成情况 
        /// </summary>
        /// <returns></returns>
        public bool CheckCompleteCondition()
        {
            double rotationPosition = _rotationAxis.MotionData.MotorPosition;
            if (!_rotationAxis.CheckPositionIsInStation(rotationPosition, "Robot"))
            {
                NotifyError(eEvent.ERR_PUF, $"rotation {rotationPosition} not in Robot", 0);
                return false;
            }
            double verticalPosition = _verticalAxis.MotionData.MotorPosition;
            if (!_verticalAxis.CheckPositionIsInStation(verticalPosition, "Robot"))
            {
                NotifyError(eEvent.ERR_PUF, $"vertical {verticalPosition} not in Robot", 0);
                return false;
            }
            return true;
        }
    }
}