CyberX8/CyberX8_RT/Modules/PUF/PufGotoRobotForPlaceRoutine.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.Utilities;
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;

namespace CyberX8_RT.Modules.PUF
{
    public class PufGotoRobotForPlaceRoutine : RoutineBase, IRoutine
    {
        private enum GotoRobotStep
        {
            VerticalFlip,
            VerticalFlipWait,
            RotationFlip,
            RotationFlipWait,
            FlipGotoSide,
            FlipGotoSideWait,
            VerticalGotoRobot,
            VerticalWait,
            RotationGotoRobot,
            RotationWait,
            End
        }
        #region 常量
        private const string VACUUM_OFF = "VacuumOff";
        #endregion

        #region 内部变量
        /// <summary>
        /// Side
        /// </summary>
        private string _side = "";
        /// <summary>
        /// Flip Axis
        /// </summary>
        private JetAxisBase _flipAxis;
        /// <summary>
        /// Rotation Axis
        /// </summary>
        private JetAxisBase _rotationAxis;
        /// <summary>
        /// Vertical Axis
        /// </summary>
        private JetAxisBase _verticalAxis;
        /// <summary>
        /// Puf Vacuum
        /// </summary>
        private PufVacuum _pufVacuum;
        /// <summary>
        /// Flip Side
        /// </summary>
        private string _flipSide;
        /// <summary>
        /// 是否需要翻转
        /// </summary>
        private bool _needFlip;
        #endregion

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public PufGotoRobotForPlaceRoutine(string module) : base(module)
        {
        }
        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(GotoRobotStep.VerticalFlip, () => { return _verticalAxis.PositionStation("Flip"); },_delay_1ms)
                .WaitWithStopCondition(GotoRobotStep.VerticalFlipWait,CheckVerticalPositionStatus,CheckVerticalPositionRunStop)
                .Run(GotoRobotStep.RotationFlip, RotationGotoFlip,_delay_1ms)
                .WaitWithStopCondition(GotoRobotStep.RotationFlipWait,CheckRotationGotoFlipStatus,CheckRotationGotoFlipStopStatus)
                .Run(GotoRobotStep.VerticalGotoRobot, () => { return _verticalAxis.PositionStation("Robot"); }, _delay_1ms)
                .Run(GotoRobotStep.FlipGotoSide, () => { return _flipAxis.PositionStation(_flipSide); }, _delay_1ms)
                .WaitWithStopCondition(GotoRobotStep.FlipGotoSideWait, CheckFlipPositionStatus, CheckFlipPositionRunStop)
                .WaitWithStopCondition(GotoRobotStep.VerticalWait, CheckVerticalPositionStatus, CheckVerticalPositionRunStop)
                .Run(GotoRobotStep.RotationGotoRobot, () => { return _rotationAxis.PositionStation("Robot"); }, _delay_1ms)
                .WaitWithStopCondition(GotoRobotStep.RotationWait, CheckRotationPositionStatus, CheckRotationPositionRunStop)
                .End(GotoRobotStep.End, NullFun, _delay_1ms);
            return Runner.Status;
        }
        /// <summary>
        /// Rotation Goto Flip
        /// </summary>
        /// <returns></returns>
        private bool RotationGotoFlip()
        {
            if(_needFlip)
            {
                return _rotationAxis.PositionStation("Flip");
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验Rotation Goto Flip状态
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationGotoFlipStatus()
        {
            if(_needFlip)
            {
                return _rotationAxis.Status == RState.End;
            }
            else
            {
                return true;
            }
        }
        /// <summary>
        /// 检验Rotation Goto Flip停止状态
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationGotoFlipStopStatus()
        {
            if(_needFlip)
            {
                return _rotationAxis.Status == RState.End;
            }
            else
            {
                return false;
            }
        }
        /// <summary>
        /// 检验Rotation移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationPositionStatus()
        {
            return _rotationAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Rotation是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckRotationPositionRunStop()
        {
            return _rotationAxis.Status == RState.Failed;
        }
        /// <summary>
        /// 检验Vertical移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckVerticalPositionStatus()
        {
            return _verticalAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Vertical是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckVerticalPositionRunStop()
        {
            return _verticalAxis.Status == RState.Failed;
        }
        /// <summary>
        /// 检验Flip移动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckFlipPositionStatus()
        {
            return _flipAxis.Status == RState.End;
        }
        /// <summary>
        /// 检验Flip是否还在运动
        /// </summary>
        /// <returns></returns>
        private bool CheckFlipPositionRunStop()
        {
            return _flipAxis.Status == RState.Failed;
        }

        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _side = objs[0].ToString();
            if(_side=="SideA")
            {
                _flipSide = "SideB";//Flip 运动至Side相反位置，即使用SideA，则Flip运动至SideB
            }
            else
            {
                _flipSide = "SideA";
            }
            _flipAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Flip");
            _rotationAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Rotation");
            _verticalAxis = DEVICE.GetDevice<JetAxisBase>($"{Module}.Vertical");
            _pufVacuum = DEVICE.GetDevice<PufVacuum>($"{Module}.Vacuum");
            if(!CheckPreCondition())
            {
                return RState.Failed;
            }
            _needFlip = !_flipAxis.CheckPositionIsInStation(_flipAxis.MotionData.MotorPosition, _flipSide);
            return Runner.Start(Module, "Go to Robot");
        }
        /// <summary>
        /// 前置条件
        /// </summary>
        /// <returns></returns>
        private bool CheckPreCondition()
        {
            if(!_flipAxis.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_PUF, Module, "Flip Axis is not homed");
                return false;
            }
            if (!_rotationAxis.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_PUF, Module, "Rotation Axis is not homed");
                return false;
            }

            if (!_verticalAxis.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_PUF, Module, "Vertical Axis is not homed");
                return false;
            }

            if (_side == "SideA")
            {
                if(_pufVacuum.ChuckAVacuumStatus!=VACUUM_OFF)
                {
                    LOG.WriteLog(eEvent.ERR_PUF, Module, "Side A is not Vacuum off");
                    return false;
                }
            }

            else if(_side=="SideB")
            {
                if (_pufVacuum.ChuckBVacuumStatus != VACUUM_OFF)
                {
                    LOG.WriteLog(eEvent.ERR_PUF, Module, "Side B is not Vacuum off");
                    return false;
                }
            }

            if(!_verticalAxis.CheckGotoPosition("Robot"))
            {
                return false;
            }

            if(!_rotationAxis.CheckGotoPosition("Robot"))
            {
                return false;
            }

            return true;
        }
    }
}