CyberX8/CyberX8_RT/Devices/LinMot/LinMotStartVAIPositionRoutine.cs

using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using MECF.Framework.Common.Device.LinMot;
using MECF.Framework.Common.Routine;
using CyberX8_Core;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;

namespace CyberX8_RT.Devices.LinMot
{
    public class LinMotStartVAIPositionRoutine : RoutineBase, IRoutine
    {
        private enum VAIPositionStep
        {
            SwitchOn,
            StartGoTopPosition,
            Delay,
            FirstWaitSendNextCommand,
            LoopGoBottomPosition,
            LoopWaitBottomCheckDirection,
            LoopGoTopPosition,
            LoopWaitTopCheckDirection,
            LoopEnd,
            LastGoBottomPosition,
            LastGoBottomPositionWait,
            CheckTopMotor,
            CheckMotor,
            LastDelay,
            SwitchOff,
            End
        }

        #region 内部变量
        /// <summary>
        /// 所有扫描次数
        /// </summary>
        private int _totalScan = 0;
        /// <summary>
        /// 当前次数
        /// </summary>
        private int _currentScan = 0;
        /// <summary>
        /// 参数数据
        /// </summary>
        private LinMotDeviceData _linMotDeviceData;
        /// <summary>
        /// 电机对象
        /// </summary>
        private LinMotAxis _axis;
        /// <summary>
        /// 上一次方向 
        /// </summary>
        private string _lastDirection = "";
        /// <summary>
        /// 最开始的方向
        /// </summary>
        private string _startDirection = "";
        #endregion

        #region 属性
        /// <summary>
        /// 当前scan次数
        /// </summary>
        public int CurrentScan { get { return _currentScan; } }
        #endregion
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public LinMotStartVAIPositionRoutine(string module,LinMotAxis axis) : base(module)
        {
            _axis = axis;
        }

        /// <summary>
        /// 手动中止
        /// </summary>
        public void Abort()
        {
            Runner.Stop("Manual Abort");
        }

        public RState Monitor()
        {
            if(_totalScan ==1 )
            {
                Runner.Run(VAIPositionStep.SwitchOn, () => { return _axis.SendOperation(LinMotOperation.SwitchOn); }
                , () => { return _axis.IsSwitchOn; },_delay_5s)
                .Run(VAIPositionStep.StartGoTopPosition, SendVAIGoTopPosition, CheckMotorOn, _delay_3s)
                .Delay(VAIPositionStep.Delay, _delay_2s)
                .Run(VAIPositionStep.LastGoBottomPosition, ContinueVAIGoBottomPosition,CheckMotorOn, _delay_3s)
                .WaitWithStopCondition(VAIPositionStep.CheckTopMotor, CheckAxisArrivedTop,
                        () => CheckAxisMotorStopStatus((int)_linMotDeviceData.TopPosition), _delay_60s)
                .WaitWithStopCondition(VAIPositionStep.CheckMotor, CheckAxisArrivedBottom,
                        () => CheckAxisMotorStopStatus((int)_linMotDeviceData.BottomPosition), _delay_60s)
               .Wait(VAIPositionStep.CheckMotor, ()=>CheckMotorOff(false))
               .Delay(VAIPositionStep.LastDelay,_delay_1s)
               .Run(VAIPositionStep.SwitchOff, () => { return _axis.SendOperation(LinMotOperation.SwitchOff); }, CheckSwitchOff, _delay_5s)
               .End(VAIPositionStep.End, NullFun, _delay_1ms);
            }
            else
            {
                Runner.Run(VAIPositionStep.SwitchOn, () => { return _axis.SendOperation(LinMotOperation.SwitchOn); }, 
                    () => { return _axis.IsSwitchOn; }, _delay_5s)
               .Run(VAIPositionStep.StartGoTopPosition, SendVAIGoTopPosition, CheckMotorOn, _delay_3s)
               .Delay(VAIPositionStep.Delay, _delay_2s)
               .LoopStart(VAIPositionStep.LoopGoBottomPosition, "Loop Scan", _totalScan - 1, ContinueVAIGoBottomPosition, _delay_1ms)
               .LoopRunWithStopStatus(VAIPositionStep.LoopWaitBottomCheckDirection, CheckDirectionChanged,
                    ()=>CheckMotorOff(true),_delay_30s)
               .LoopRun(VAIPositionStep.LoopGoTopPosition, ContinueVAIGoTopPosition, _delay_1ms)
               .LoopRunWithStopStatus(VAIPositionStep.LoopWaitTopCheckDirection, CheckDirectionChanged,
                    ()=>CheckMotorOff(true), _delay_30s)
               .LoopEnd(VAIPositionStep.LoopEnd, NullFun, _delay_1ms)
               .Run(VAIPositionStep.LastGoBottomPosition, ContinueVAIGoBottomPosition,CheckMotorOn, _delay_3s)
               .WaitWithStopCondition(VAIPositionStep.LastGoBottomPositionWait, CheckAxisArrivedBottom,
                    ()=>CheckAxisMotorStopStatus((int)_linMotDeviceData.BottomPosition), _delay_60s)
               .Wait(VAIPositionStep.CheckMotor,()=>CheckMotorOff(false))
               .Delay(VAIPositionStep.LastDelay, _delay_1s)
               .Run(VAIPositionStep.SwitchOff, () => { return _axis.SendOperation(LinMotOperation.SwitchOff); }, CheckSwitchOff, _delay_5s)
               .End(VAIPositionStep.End, NullFun, _delay_1ms);
            }
            return Runner.Status;
        }
        /// <summary>
        /// 前置条件
        /// </summary>
        /// <returns></returns>
        private bool CheckPreCondition()
        {
            if(!_axis.IsHomed)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module, "axis is not homed");
                return false;
            }
            if (!_axis.IsConnectd)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module, "axis is not connected");
                return false;
            }
            if(_axis.IsError)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module, "axis is in error");
                return false;
            }
            return true;
        }
        /// <summary>
        /// 第一次Goto Top
        /// </summary>
        /// <returns></returns>
        private bool SendVAIGoTopPosition()
        {
            if(!_axis.IsConnectd)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module, "axis is not connected");
                return false;
            }
            _lastDirection= _axis.Direction;
            _currentScan++;
            return _axis.SendVAIGoToPosition((int)_linMotDeviceData.TopPosition, (int)_linMotDeviceData.UpMaxSpeed, _linMotDeviceData.UpMaxAcceleration,
                _linMotDeviceData.UpMaxDeceleration);
        }

        /// <summary>
        /// 检验Axis运动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckMotorOn()
        {
            return _axis.IsMotorOn;
        }
        /// <summary>
        /// 检验Axis运动状态
        /// </summary>
        /// <returns></returns>
        private bool CheckMotorOff(bool showError)
        {
            bool result=  !_axis.IsMotorOn;
            if (result)
            {
                if (showError)
                {
                    LOG.WriteLog(eEvent.ERR_LINMOT, Module, "Linmot is stop");
                }
            }
            return result;
        }
        /// <summary>
        /// 检验电机停止状态
        /// </summary>
        /// <returns></returns>
        private bool CheckAxisMotorStopStatus(int targetPosition)
        {
            bool arrived = CheckAxisArrived(targetPosition);
            if (!arrived)
            {
                return CheckMotorOff(true);
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// 检验方向是否发生变化
        /// </summary>
        /// <returns></returns>
        private bool CheckDirectionChanged()
        {
            bool result = _lastDirection != _axis.Direction;
            if(result)
            {
                _lastDirection = _axis.Direction;
                if(_lastDirection==_startDirection)
                {
                    _currentScan++;
                }
            }
            return result;
        }
        /// <summary>
        /// 不停止Go Bottom
        /// </summary>
        /// <returns></returns>
        private bool ContinueVAIGoBottomPosition()
        {
            if (!_axis.IsConnectd)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module, "axis is not connected");
                return false;
            }

            bool result = _axis.IsMotorOn;
            if (!result)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module, "axis is stopped");
                return false;
            }
            return _axis.SendGAIGoToPositionAfterActualCommand((int)_linMotDeviceData.BottomPosition, (int)_linMotDeviceData.DownMaxSpeed,
                _linMotDeviceData.DownMaxAcceleration, _linMotDeviceData.DownMaxDeceleration);
        }
        /// <summary>
        /// 检验电机是还到达顶部
        /// </summary>
        /// <returns></returns>
        private bool CheckAxisArrivedTop()
        {
            return CheckAxisArrived((int)_linMotDeviceData.TopPosition);
        }
        /// <summary>
        /// 检验电机是还到达底部
        /// </summary>
        /// <returns></returns>
        private bool CheckAxisArrivedBottom()
        {
            return CheckAxisArrived((int)_linMotDeviceData.BottomPosition);
        }

        private bool CheckAxisArrived(int targetPosition)
        {
            double bias = Math.Abs(_axis.CurrentIntPosition - targetPosition) / Math.Abs(_linMotDeviceData.TopPosition - _linMotDeviceData.BottomPosition);
            return bias <= 0.1;
        }
        /// <summary>
        /// 不停止Go Top
        /// </summary>
        /// <returns></returns>
        private bool ContinueVAIGoTopPosition()
        {
            if (!_axis.IsConnectd)
            {
                LOG.WriteLog(eEvent.ERR_LINMOT, Module, "axis is not connected");
                return false;
            }
            return _axis.SendGAIGoToPositionAfterActualCommand((int)_linMotDeviceData.TopPosition, (int)_linMotDeviceData.UpMaxSpeed, _linMotDeviceData.UpMaxAcceleration,
                _linMotDeviceData.UpMaxDeceleration);
        }
        /// <summary>
        /// 检验是否SwitchOff
        /// </summary>
        /// <returns></returns>
        private bool CheckSwitchOff()
        {
            return !_axis.IsSwitchOn;
        }
        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _totalScan=(int)objs[0];
            _linMotDeviceData=(LinMotDeviceData)objs[1];
            _lastDirection = _axis.Direction;
            _startDirection = _axis.Direction;
            _currentScan = 0;        
            if(!CheckPreCondition())
            {
                return RState.Failed;
            }
            Runner.Start(Module, "VAI Go to Position");
            return RState.Running;
        }

        
    }
}