CyberX8/CyberX8_RT/Modules/Reservoir/ANTransferPumpOnRoutine.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using CyberX8_Core;
using CyberX8_RT.Devices.Reservoir;
using MECF.Framework.Common.Beckhoff.ModuleIO;
using MECF.Framework.Common.IOCore;
using MECF.Framework.Common.Persistent.Reservoirs;
using MECF.Framework.Common.Routine;
using MECF.Framework.Common.TwinCat;
using System;

namespace CyberX8_RT.Modules.Reservoir
{
    public class ANTransferPumpOnRoutine : RoutineBase, IRoutine
    {
        private enum ANTransferPumpStep
        {
            CrossDoseValveOn,
            Reset,
            WaitReset,
            CheckReadyToEnable,
            Enable,
            CheckReady,
            WriteParameter,
            Execute,
            Wait,
            Done,
            End
        }
        private enum STMStatus
        {
            ReadyToEnable,
            Ready,
            Warning,
            Error
        }
        private enum POSStatus
        {
            Busy,
            InTarget,
            Warning,
            Error
        }
        private enum ENCStatus
        {
            SetCounterDone            
        }

        #region 常量
        private const string TRANSFER_PUMP_ENABLE = "TransferPumpEnable";
        private const string TRANSFER_PUMP_RESET = "TransferPumpReset";
        private const string TRANSFER_PUMP_EXECUTE = "TransferPumpExecute";
        private const string TRANSFER_PUMP_TARGET_POSITION = "TransferPumpTargetPosition";
        private const string TRANSFER_PUMP_SPEED = "TransferPumpSpeed";
        private const string TRANSFER_PUMP_START_TYPE = "TransferPumpStartType";
        private const string TRANSFER_PUMP_ACCELERATION = "TransferPumpAcceleration";
        private const string TRANSFER_PUMP_DECELERATION = "TransferPumpDeceleration";
        private const string CROSS_DOSE_ENABLE = "CrossDoseEnable";
        #endregion

        #region 内部变量
        /// <summary>
        /// Module Name
        /// </summary>
        private string _moduleName;
        /// <summary>
        /// CompactMembranReservoirDevice 
        /// </summary>
        private CompactMembranReservoirDevice _cmReservoirDevice;
        /// <summary>
        /// 加速度
        /// </summary>
        private int _acceleration;
        /// <summary>
        /// 减速度
        /// </summary>
        private int _deceleration;
        /// <summary>
        /// 速度
        /// </summary>
        private int _speed;
        /// <summary>
        /// 目标位置
        /// </summary>
        private double _targetPosition;
        /// <summary>
        /// 开始位置
        /// </summary>
        private double _startPosition;
        /// <summary>
        /// Start Type
        /// </summary>
        private int _startType = 1; //RELATIVE（相对位置）
        /// <summary>
        /// ReservoirsPersistentValue
        /// </summary>
        private ReservoirsPersistentValue _persistentValue;
        /// <summary>
        /// 超时时间
        /// </summary>
        private int _timeOut = 2000;
        /// <summary>
        /// PumpFactor
        /// </summary>
        private double _pumpFactor;
        /// <summary>
        /// 最小流量
        /// </summary>
        private double _flowRateMin;
        /// <summary>
        /// StartUpHoldOffTime    
        /// </summary>
        private int _startUpHoldOffTime;
        /// <summary>
        /// Pump启动时刻
        /// </summary>
        private int _startTime;
        #endregion

        #region 属性
        public RState CurrentState { get { return Runner.Status; } }
        #endregion

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="module"></param>
        public ANTransferPumpOnRoutine(string module) : base(module)
        {
            _moduleName = module;
        }

        /// <summary>
        /// 中止
        /// </summary>
        public void Abort()
        {

            Runner.Stop("Manual abort");
        }
        /// <summary>
        /// 监控
        /// </summary>
        /// <returns></returns>
        public RState Monitor()
        {
            Runner.Run(ANTransferPumpStep.CrossDoseValveOn, OpenValve, _delay_1ms)
                .Run(ANTransferPumpStep.Reset, ResetCrossDose, _delay_1ms)
                .Wait(ANTransferPumpStep.WaitReset, WaitReset, _timeOut)
                .Run(ANTransferPumpStep.CheckReadyToEnable, CheckReadyToEnable, _delay_1ms)
                .Run(ANTransferPumpStep.Enable, EnableOperation, _delay_1ms)
                .Run(ANTransferPumpStep.CheckReady, CheckReady, _delay_1ms)
                .Run(ANTransferPumpStep.WriteParameter, WriteParameter, _delay_1ms)
                .Run(ANTransferPumpStep.Execute, ExecuteOperation, _delay_1ms)
                .WaitWithStopCondition(ANTransferPumpStep.Wait, WaitCompleteEndStatus, WaitCompleteErrorStatus)
                .Run(ANTransferPumpStep.Done, DoneOperation, _delay_1ms)
                .End(ANTransferPumpStep.End, NullFun);
            return Runner.Status;
        }

        /// <summary>
        /// 启动
        /// </summary>
        /// <param name="objs"></param>
        /// <returns></returns>
        public RState Start(params object[] objs)
        {
            _startType = 2;
            _cmReservoirDevice = DEVICE.GetDevice<CompactMembranReservoirDevice>($"{_moduleName}");
            if (_cmReservoirDevice == null)
            {
                LOG.WriteLog(eEvent.ERR_RESERVOIR, _moduleName, "Compact Membran Reservoir Device is not exist");
                return RState.Failed;
            }
            _persistentValue = ReservoirsPersistentManager.Instance.GetReservoirsPersistentValue(_moduleName);
            if (_persistentValue == null)
            {
                LOG.WriteLog(eEvent.ERR_RESERVOIR, _moduleName, "Reservoir Persistent Value Object is not exist");
                return RState.Failed;
            }
            //加速度(count/s/s)
            if (SC.ContainsItem($"Reservoir.{Module}.ANTransferPumpAcceleration"))
            {
                _acceleration = SC.GetValue<int>($"Reservoir.{Module}.ANTransferPumpAcceleration");
                if (_acceleration == 0) return RState.Failed;
            }
            //减速度(count/s/s)
            if (SC.ContainsItem($"Reservoir.{Module}.ANTransferPumpDeceleration"))
            {
                _deceleration = SC.GetValue<int>($"Reservoir.{Module}.ANTransferPumpDeceleration");
                if (_deceleration == 0) return RState.Failed;
            }
            //速度(count/s)
            if (SC.ContainsItem($"Reservoir.{Module}.ANTransferPumpSpeed"))
            {
                _speed = SC.GetValue<int>($"Reservoir.{Module}.ANTransferPumpSpeed");
                if (_speed == 0) return RState.Failed;
            }
            //目标位置(count)（1mL = 23809.96272 counts）
            _startPosition = _cmReservoirDevice.ReservoirData.TransferActualPosition;
            _targetPosition = (double)objs[0];
            _pumpFactor = _persistentValue.CrossDosePumpFactor;
            if(_pumpFactor == 0)
            {
                LOG.WriteLog(eEvent.WARN_RESERVOIR, _moduleName, "Pump Factor is zero");
                return RState.Failed;
            }
            //最小FlowRate（mL/min）
            if (SC.ContainsItem($"Reservoir.{Module}.FlowRateMin"))
            {
                _flowRateMin = SC.GetValue<double>($"Reservoir.{Module}.FlowRateMin");
            }
            //StartUpHoldOffTime(流量检测时刻,ms)
            if (SC.ContainsItem($"Reservoir.{Module}.StartUpHoldOffTime"))
            {
                _startUpHoldOffTime = SC.GetValue<int>($"Reservoir.{Module}.StartUpHoldOffTime");
            }
            return Runner.Start(Module, "AN Transfer Pump On");
        }
        /// <summary>
        /// 打开CrossDose Valve
        /// </summary>
        /// <returns></returns>
        private bool OpenValve() 
        {
            if (!_cmReservoirDevice.CrossDoseOn("", null))
            {
                LOG.WriteLog(eEvent.WARN_RESERVOIR, _moduleName, $"Close CrossDose Valve failed");
                return false;
            }
            return true;    
        }
        /// <summary>
        /// Reset CrossDose
        /// </summary>
        /// <returns></returns>
        private bool ResetCrossDose()
        {
            return _cmReservoirDevice.ResetCrossDose("", null);
        }
        /// <summary>
        /// Reset CrossDose
        /// </summary>
        /// <returns></returns>
        private bool WaitReset()
        {
            if (_cmReservoirDevice.ResetCrossDoseMonitor())
            {
                return true;
            }
            return false;
        }
        /// <summary>
        /// 检查电机ReadyToEnable状态
        /// </summary>
        /// <returns></returns>
        private bool CheckReadyToEnable()
        {
            //STM：ReadyToEnable = 1
            ushort status = (ushort)_cmReservoirDevice.ReservoirData.TransferPumpSTMStatus;
            if (!CheckSTMStatus(status, STMStatus.ReadyToEnable))
            {
                LOG.WriteLog(eEvent.ERR_RESERVOIR, _moduleName, $"STMStatus: {status}. Ready to enable is false");
                return false;
            }
            
            return true;
        }
        /// <summary>
        /// EnableOperation
        /// </summary>
        /// <returns></returns>
        private bool EnableOperation()
        {
            string ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_ENABLE}");
            bool result = IOModuleManager.Instance.WriteIoValue(ioName, true);
            if (!result)
            {
                return false;
            }
            return true;
        }
        /// <summary>
        /// 检查电机Ready状态
        /// </summary>
        /// <returns></returns>
        private bool CheckReady()
        {
            //STM：Ready = 1
            ushort status = (ushort)_cmReservoirDevice.ReservoirData.TransferPumpSTMStatus;
            if (!CheckSTMStatus(status, STMStatus.Ready))
            {
                LOG.WriteLog(eEvent.ERR_RESERVOIR, _moduleName, $"STMStatus: {status}. Ready is false");
                return false;
            }
            return true;
        }
        /// <summary>
        /// 设置电机参数
        /// </summary>
        /// <returns></returns>
        private bool WriteParameter()
        {
            //设置加速度
            string ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_ACCELERATION}");
            bool result = IOModuleManager.Instance.WriteIoValue(ioName, _acceleration);
            if (!result)
            {
                return false;
            }
            //设置减速度
            ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_DECELERATION}");
            result = IOModuleManager.Instance.WriteIoValue(ioName, _deceleration);
            if (!result)
            {
                return false;
            }
            //设置速度
            ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_SPEED}");
            result = IOModuleManager.Instance.WriteIoValue(ioName, _speed);
            if (!result)
            {
                return false;
            }
            //设置StartType
            ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_START_TYPE}");
            result = IOModuleManager.Instance.WriteIoValue(ioName, _startType);
            if (!result)
            {
                return false;
            }
            //设置目标位置(mL)
            ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_TARGET_POSITION}");
            result = IOModuleManager.Instance.WriteIoValue(ioName, _targetPosition/ _pumpFactor);
            if (!result)
            {
                return false;
            }
            return true;
        }
        /// <summary>
        /// 运动到位置
        /// </summary>
        /// <returns></returns>
        private bool ExecuteOperation()
        {
            string ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_EXECUTE}");
            bool result = IOModuleManager.Instance.WriteIoValue(ioName, true);
            if (!result)
            {
                return false;
            }
            _startTime = Environment.TickCount;
            ReservoirsPersistentManager.Instance.UpdatePersistentValue(_moduleName);
            return true;
        }
        
        /// <summary>
        ///  WaitCompleteEnd
        /// </summary>
        /// <returns></returns>
        private bool WaitCompleteEndStatus()
        {
            ushort status = (ushort)_cmReservoirDevice.ReservoirData.TransferPumpPOSStatus;
            double dosedVolume = (_cmReservoirDevice.ReservoirData.TransferActualPosition - _startPosition) * _pumpFactor;
            _cmReservoirDevice.ReservoirData.RemainingCrossDoseVolume = Math.Round(_targetPosition - dosedVolume,2);
            if (CheckPOSStatus(status, POSStatus.InTarget) && !CheckPOSStatus(status, POSStatus.Busy))
            {
                return true;
            }
            return false;
            
        }
        /// <summary>
        ///  WaitCompleteError
        /// </summary>
        /// <returns></returns>
        private bool WaitCompleteErrorStatus()
        {
            ushort status = (ushort)_cmReservoirDevice.ReservoirData.TransferPumpPOSStatus;
            //流量检测
            if(_cmReservoirDevice.ANTransferFlow != null)
            {
                int ticks = Environment.TickCount - _startTime;
                double flow = _cmReservoirDevice.ANTransferFlow.CounterValue;
                if (ticks >= _startUpHoldOffTime && flow < _flowRateMin)
                {
                    LOG.WriteLog(eEvent.ERR_RESERVOIR, _moduleName, $"Dose Flow {flow} mL/min is not over {_flowRateMin} mL/min");
                    return true;
                }
            }
            
            if (CheckPOSStatus(status, POSStatus.Error))
            {
                LOG.WriteLog(eEvent.WARN_RESERVOIR, _moduleName, $"POSStatus:{status}. Error is true");
                return true;
            }
            return false;
        }
        /// <summary>
        /// DoneOperation
        /// </summary>
        /// <returns></returns>
        private bool DoneOperation()
        {
            string ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_EXECUTE}");
            bool result = IOModuleManager.Instance.WriteIoValue(ioName, false);
            if (!result)
            {
                return false;
            }
            ioName = BeckhoffModuleIOManager.Instance.GetIoNameByInnerModuleName($"{Module}.{TRANSFER_PUMP_ENABLE}");
            result = IOModuleManager.Instance.WriteIoValue(ioName, false);
            if (!result)
            {
                return false;
            }
            if (!_cmReservoirDevice.CrossDoseOff("", null))
            {
                LOG.WriteLog(eEvent.WARN_RESERVOIR, _moduleName, $"Close CrossDose Valve failed");
                return false;
            }
            _persistentValue.AutoCrossDoseStartTime = DateTime.MinValue;
            _persistentValue.AutoCrossDoseStartAmpHour = 0;
            _cmReservoirDevice.ReservoirData.RemainingCrossDoseVolume = 0;
            ReservoirsPersistentManager.Instance.UpdatePersistentValue(_moduleName);
            return true;
        }
        /// <summary>
        /// STM状态判断
        /// </summary>
        /// <param name="status"></param>
        /// <param name="statusName"></param>
        /// <returns></returns>
        private bool CheckSTMStatus(ushort status, STMStatus statusName)
        {
            bool result = false;
            switch (statusName)
            {
                case STMStatus.ReadyToEnable:
                    byte bit0 = (byte)(status & 0b0001);
                    result = (bit0 == 1) ? true : false;
                    break;
                case STMStatus.Ready:
                    byte bit1 = (byte)((status & 0b0010) >> 1);
                    result = (bit1 == 1) ? true : false;
                    break;
                case STMStatus.Warning:
                    byte bit2 = (byte)((status & 0b0100) >> 2);
                    result = (bit2 == 1) ? true : false;
                    break;
                case STMStatus.Error:
                    byte bit3 = (byte)((status & 0b1000) >> 3);
                    result = (bit3 == 1) ? true : false;
                    break;
                default:
                    break;
            }
            return result;
        }
        /// <summary>
        /// POS状态判断
        /// </summary>
        /// <param name="status"></param>
        /// <param name="statusName"></param>
        /// <returns></returns>
        private bool CheckPOSStatus(ushort status, POSStatus statusName)
        {
            bool result = false;
            switch (statusName)
            {
                case POSStatus.Busy:
                    byte bit0 = (byte)(status & 0b0001);
                    result = (bit0 == 1) ? true : false;
                    break;
                case POSStatus.InTarget:
                    byte bit1 = (byte)((status & 0b0010) >> 1);
                    result = (bit1 == 1) ? true : false;
                    break;
                case POSStatus.Warning:
                    byte bit2 = (byte)((status & 0b0100) >> 2);
                    result = (bit2 == 1) ? true : false;
                    break;
                case POSStatus.Error:
                    byte bit3 = (byte)((status & 0b1000) >> 3);
                    result = (bit3 == 1) ? true : false;
                    break;
                default:
                    break;
            }
            return result;
        }
    }
}