Virgo_Mask/VirgoRT/Devices/IODevices/IoTriStateLift4.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.IOCore;

using System.Xml;
using System.Diagnostics;

using Aitex.Core.Common.DeviceData;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.SCCore;
using Aitex.Core.RT.OperationCenter;

using VirgoCommon;
using Aitex.Core.Util;
using System;

namespace VirgoRT.Devices
{
    public class IoTriStateLift4 : BaseDevice, IDevice
    {
        private readonly DIAccessor _diOrigin;//是否到达原点
        private readonly DIAccessor _diP1;//是否到达位置1
        private readonly DIAccessor _diP2;//是否到达位置2
        private readonly DIAccessor _diP3;//是否到达位置3

        private readonly DIAccessor _diABSAlarm;//去位置1，2，3超时
        private readonly DIAccessor _diORGAlarm;//去原点超时
        //private readonly DIAccessor _diServoAlarm; // servo alarm
        //private readonly DIAccessor _diCCWLimitSensorAlarm;  // 
        private readonly DIAccessor _diOverSoftwareLimitAlarm; // 

        private readonly DOAccessor _doReset;

        private readonly DOAccessor _doOrigin;//去原点
        private readonly DOAccessor _doP1;//去位置1
        private readonly DOAccessor _doP2;//去位置2
        private readonly DOAccessor _doP3;//去位置3

        private readonly DOAccessor _doStop;
        private readonly DOAccessor _doUp;
        private readonly DOAccessor _doDown;


        private readonly DOAccessor _doServo1JogUP;
        private readonly DOAccessor _doServo1JogDOWN;
        private readonly DOAccessor _doServo1Stop;
        private readonly DOAccessor _doServo1StepUp;
        private readonly DOAccessor _doServo1StepDOWN;

        private readonly DOAccessor _doServo2JogUP;
        private readonly DOAccessor _doServo2JogDOWN;
        private readonly DOAccessor _doServo2Stop;
        private readonly DOAccessor _doServo2StepUp;
        private readonly DOAccessor _doServo2StepDOWN;

        private readonly DOAccessor _doServo3JogUP;
        private readonly DOAccessor _doServo3JogDOWN;
        private readonly DOAccessor _doServo3Stop;
        private readonly DOAccessor _doServo3StepUp;
        private readonly DOAccessor _doServo3StepDOWN;

        private readonly DOAccessor _doServo4JogUP;
        private readonly DOAccessor _doServo4JogDOWN;
        private readonly DOAccessor _doServo4Stop;
        private readonly DOAccessor _doServo4StepUp;
        private readonly DOAccessor _doServo4StepDOWN;


        private readonly AIAccessor _currentValue;
        private readonly AIAccessor _currentSpeed;//aiCurrentSpeed


        private readonly AIAccessor _aiMotor1CurrentPosition;
        private readonly AIAccessor _aiMotor2CurrentPosition;
        private readonly AIAccessor _aiMotor3CurrentPosition;
        private readonly AIAccessor _aiMotor4CurrentPosition;
        private readonly AIAccessor _aiServo1AlarmCode;
        private readonly AIAccessor _aiServo2AlarmCode;
        private readonly AIAccessor _aiServo3AlarmCode;
        private readonly AIAccessor _aiServo4AlarmCode;
        private readonly AIAccessor _aiServoSVRAlarmCode;
        private readonly AIAccessor _aiServo1WarningCode;
        private readonly AIAccessor _aiServo2WarningCode;
        private readonly AIAccessor _aiServo3WarningCode;
        private readonly AIAccessor _aiServo4WarningCode;
        private readonly AIAccessor _aiServoSVRWarningCode;



        private readonly AOAccessor _aoSetP1;
        private readonly AOAccessor _aoSetP2;
        private readonly AOAccessor _aoSetP3;

        //private readonly AOAccessor _aoServoEnable;
        private readonly AOAccessor _aoAutoSpeed;
        private readonly AOAccessor _aoManualSpeed;
        private readonly AOAccessor _aoAccOrDecTime;
        //private readonly AOAccessor _aoDecTime;
        private readonly AOAccessor _aoCCWLimitPosition;
        private readonly AOAccessor _aoCWLimitPosition;
        //private readonly AOAccessor _aoCorrectionValue;

        private readonly AOAccessor _aoStepDistance;
        private readonly AOAccessor _aoWorkMode;
        private readonly AOAccessor _aoSingleAxisManualSpeed;
        private readonly AOAccessor _aoOriginOffset;

        private Stopwatch sw = new Stopwatch();
        private Stopwatch _manualStopTimer = new Stopwatch();
        private readonly int _stopButtonAutoResetTime = 1000;

        private Position _currentTarget = Position.Invalid;

        long _timeout = 10000;
        private bool _bAlarmReported = false;


        private AITTriStateLiftPinData DeviceData
        {
            get
            {
                AITTriStateLiftPinData deviceData = new AITTriStateLiftPinData
                {
                    Module = Module,
                    DeviceName = Name,
                    DeviceSchematicId = DeviceID,
                    DisplayName = Display,

                };

                return deviceData;
            }
        }

        public MovementPosition PinPosition
        {
            get
            {
                if (_diP1.Value && _diP2.Value == false && _diP3.Value == false)
                    return MovementPosition.Up;
                else if (_diP1.Value == false && _diP2.Value && _diP3.Value == false)
                    return MovementPosition.Middle;
                else if (_diP1.Value == false && _diP2.Value == false && _diP3.Value)
                    return MovementPosition.Down;
                else if (_diOrigin.Value)
                    return MovementPosition.Origin;

                return MovementPosition.Unknown;
            }
        }

        public int PinPositionint
        {
            get
            {
                if (PinPosition == MovementPosition.Up)
                    return 3;
                else if (PinPosition == MovementPosition.Middle)
                    return 2;
                else if (PinPosition == MovementPosition.Down)
                    return 1;
                else if (PinPosition == MovementPosition.Origin)
                    return 0;

                return -1;
            }
        }

        public IoTriStateLift4(string module, XmlElement node, string ioModule = "")
        {
            base.Module = module;
            base.Name = node.GetAttribute("id");
            base.Display = node.GetAttribute("display");
            base.DeviceID = node.GetAttribute("schematicId");


            _diOrigin = ParseDiNode("diOrigin", node, ioModule);

            _diP1 = ParseDiNode("diP1", node, ioModule);
            _diP2 = ParseDiNode("diP2", node, ioModule);
            _diP3 = ParseDiNode("diP3", node, ioModule);

            _diABSAlarm = ParseDiNode("diABSAlarm", node, ioModule);
            _diORGAlarm = ParseDiNode("diORGAlarm", node, ioModule);
            _diOverSoftwareLimitAlarm = ParseDiNode("diOverSoftwareLimitAlarm", node, ioModule);

            //_diServoAlarm = ParseDiNode("diServoAlarm", node, ioModule);
            //_diCCWLimitSensorAlarm = ParseDiNode("diCCWLimitAlarm", node, ioModule);


            _doReset = ParseDoNode("doReset", node, ioModule);

            _doOrigin = ParseDoNode("doOrigin", node, ioModule);
            _doP1 = ParseDoNode("doP1", node, ioModule);
            _doP2 = ParseDoNode("doP2", node, ioModule);
            _doP3 = ParseDoNode("doP3", node, ioModule);

            _doStop = ParseDoNode("doStop", node, ioModule);
            _doUp = ParseDoNode("doUp", node, ioModule);
            _doDown = ParseDoNode("doDown", node, ioModule);
            _currentValue = ParseAiNode("aiCurrentValue", node, ioModule);
            _currentSpeed = ParseAiNode("aiCurrentSpeed", node, ioModule);

            _aiMotor1CurrentPosition = ParseAiNode("aiMotor1CurrentPosition", node, ioModule);
            _aiMotor2CurrentPosition = ParseAiNode("aiMotor2CurrentPosition", node, ioModule);
            _aiMotor3CurrentPosition = ParseAiNode("aiMotor3CurrentPosition", node, ioModule);
            _aiMotor4CurrentPosition = ParseAiNode("aiMotor4CurrentPosition", node, ioModule);

            _aiServo1AlarmCode = ParseAiNode("aiServo1AlarmCode", node, ioModule);
            _aiServo2AlarmCode = ParseAiNode("aiServo2AlarmCode", node, ioModule);
            _aiServo3AlarmCode = ParseAiNode("aiServo3AlarmCode", node, ioModule);
            _aiServo4AlarmCode = ParseAiNode("aiServo4AlarmCode", node, ioModule);
            _aiServoSVRAlarmCode = ParseAiNode("aiServoSVRAlarmCode", node, ioModule);

            _aiServo1WarningCode = ParseAiNode("aiServo1WarningCode", node, ioModule);
            _aiServo2WarningCode = ParseAiNode("aiServo2WarningCode", node, ioModule);
            _aiServo3WarningCode = ParseAiNode("aiServo3WarningCode", node, ioModule);
            _aiServo4WarningCode = ParseAiNode("aiServo4WarningCode", node, ioModule);
            _aiServoSVRWarningCode = ParseAiNode("aiServoSVRWarningCode", node, ioModule);

            _aoSetP1 = ParseAoNode("aoSetP1", node, ioModule);
            _aoSetP2 = ParseAoNode("aoSetP2", node, ioModule);
            _aoSetP3 = ParseAoNode("aoSetP3", node, ioModule);

            //_aoServoEnable = ParseAoNode("aoServoEnable", node, ioModule);
            _aoAutoSpeed = ParseAoNode("aoAutoSpeed", node, ioModule);
            _aoManualSpeed = ParseAoNode("aoManualSpeed", node, ioModule);
            _aoAccOrDecTime = ParseAoNode("aoAccOrDecTime", node, ioModule);
            //_aoDecTime = ParseAoNode("aoDecTime", node, ioModule);
            _aoCCWLimitPosition = ParseAoNode("aoCCWLimitPosition", node, ioModule);
            _aoCWLimitPosition = ParseAoNode("aoCWLimitPosition", node, ioModule);
            //_aoCorrectionValue = ParseAoNode("aoCorrectionValue", node, ioModule);

            _aoStepDistance = ParseAoNode("aoStepDistance", node, ioModule);
            _aoWorkMode = ParseAoNode("aoWorkMode", node, ioModule);
            _aoSingleAxisManualSpeed = ParseAoNode("aoSingleAxisManualSpeed", node, ioModule);
            _aoOriginOffset = ParseAoNode("aoOriginOffset", node, ioModule);

            _doServo1JogUP = ParseDoNode("doServo1JogUP", node, ioModule);
            _doServo1JogDOWN = ParseDoNode("doServo1JogDOWN", node, ioModule);
            _doServo1Stop = ParseDoNode("doServo1Stop", node, ioModule);
            _doServo1StepUp = ParseDoNode("doServo1StepUp", node, ioModule);
            _doServo1StepDOWN = ParseDoNode("doServo1StepDOWN", node, ioModule);

            _doServo2JogUP = ParseDoNode("doServo2JogUP", node, ioModule);
            _doServo2JogDOWN = ParseDoNode("doServo2JogDOWN", node, ioModule);
            _doServo2Stop = ParseDoNode("doServo2Stop", node, ioModule);
            _doServo2StepUp = ParseDoNode("doServo2StepUp", node, ioModule);
            _doServo2StepDOWN = ParseDoNode("doServo2StepDOWN", node, ioModule);

            _doServo3JogUP = ParseDoNode("doServo3JogUP", node, ioModule);
            _doServo3JogDOWN = ParseDoNode("doServo3JogDOWN", node, ioModule);
            _doServo3Stop = ParseDoNode("doServo3Stop", node, ioModule);
            _doServo3StepUp = ParseDoNode("doServo3StepUp", node, ioModule);
            _doServo3StepDOWN = ParseDoNode("doServo3StepDOWN", node, ioModule);

            _doServo4JogUP = ParseDoNode("doServo4JogUP", node, ioModule);
            _doServo4JogDOWN = ParseDoNode("doServo4JogDOWN", node, ioModule);
            _doServo4Stop = ParseDoNode("doServo4Stop", node, ioModule);
            _doServo4StepUp = ParseDoNode("doServo4StepUp", node, ioModule);
            _doServo4StepDOWN = ParseDoNode("doServo4StepDOWN", node, ioModule);

            _timeout = (int)SC.GetValue<int>($"{Module}.LiftUpDownTimeout") * 1000;
        }

        private void updatePinCfg()
        {
            //  AO-27, Lift  Servo Enable:	0=Lift Pin ,1=Lift  Servo
            //_SetRealFloat(_aoServoEnable, 1);

            void _updateItem(string data, AOAccessor ao)
            {
                var value = (float)SC.GetValue<double>($"{Module}.{Name}.{data}");
                _SetRealFloat(ao, value);
            }

            _updateItem("AutoSpeed", _aoAutoSpeed);
            _updateItem("ManualSpeed", _aoManualSpeed);
            _updateItem("AccOrDecTime", _aoAccOrDecTime);
            _updateItem("CCWLimitPosition", _aoCCWLimitPosition);
            _updateItem("CWLimitPosition", _aoCWLimitPosition);
            //_updateItem("CorrectionValue", _aoCorrectionValue);
            _updateItem("Position1", _aoSetP1);
            _updateItem("Position2", _aoSetP2);
            _updateItem("Position3", _aoSetP3);
            _updateItem("StepDistance", _aoStepDistance);
            _updateItem("WorkMode", _aoWorkMode);
            _updateItem("SingleAxisManualSpeed", _aoSingleAxisManualSpeed);
            _updateItem("OriginOffset", _aoOriginOffset);

        }
        public bool GoPosition(Position position)
        {
            if (_diABSAlarm.Value)
                return false;

            _currentTarget = position;
            sw.Restart();
            switch (position)
            {
                case Position.position1:
                    _doP1.Value = true;
                    _doP2.Value = false;
                    _doP3.Value = false;
                    break;

                case Position.position2:
                    _doP1.Value = false;
                    _doP2.Value = true;
                    _doP3.Value = false;
                    break;

                case Position.position3:
                    _doP1.Value = false;
                    _doP2.Value = false;
                    _doP3.Value = true;
                    break;
                case Position.origin:
                    {
                        if (_diOrigin.Value)
                        {
                            EV.PostInfoLog(Module, $"Lift Pin already on original position.");
                            sw.Stop();
                            return true;
                        }
                        _doOrigin.Value = true;
                    }
                    break;
            }

            EV.PostInfoLog(Module, $"Lift Pin goto {_currentTarget}");

            return true;
        }

        public bool ManulStop()
        {
            if (_diABSAlarm.Value)
                return false;
            _doUp.Value = false;
            _doDown.Value = false;
            _doStop.Value = true;
            _manualStopTimer.Restart();
            return true;
        }

        public bool ManulUp()
        {
            if (_diABSAlarm.Value)
                return false;

            _doDown.Value = false;
            _doStop.Value = false;
            _doUp.Value = !_doUp.Value;

            return true;
        }

        public bool ManulDown()
        {
            if (_diABSAlarm.Value)
                return false;
            _doUp.Value = false;
            _doStop.Value = false;
            _doDown.Value = !_doDown.Value;

            return true;
        }

        public bool ServoOP(DOAccessor doTrue, params DOAccessor[] otherDoFalses)
        {
            if (_diABSAlarm.Value)
                return false;

            doTrue.Value = true;
            Array.ForEach(otherDoFalses, p => p.Value = false);

            return true;
        }

        public float CurrentValue
        {
            get
            {
                if (_currentValue == null)
                {
                    return 0;
                }

                return _GetRealFloat(_currentValue); ;
            }
        }

        public bool Initialize()
        {
            DATA.Subscribe($"{Module}.{Name}.DeviceData", () => DeviceData, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.PinPosition", () => PinPositionint, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.CurrentValue", () => CurrentValue, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.ServoWithOrigin", () => _diOrigin.Value, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.CurrentSpeed", () => _GetRealFloat(_currentSpeed), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.ManualStopState", () => _doStop.Value, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.ManualUpState", () => _doUp.Value, SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.ManualDownState", () => _doDown.Value, SubscriptionAttribute.FLAG.IgnoreSaveDB);

            DATA.Subscribe($"{Module}.{Name}.Motor1CurrentPosition", () => _GetRealFloat(_aiMotor1CurrentPosition), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Motor2CurrentPosition", () => _GetRealFloat(_aiMotor2CurrentPosition), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Motor3CurrentPosition", () => _GetRealFloat(_aiMotor3CurrentPosition), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Motor4CurrentPosition", () => _GetRealFloat(_aiMotor4CurrentPosition), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo1AlarmCode", () => _GetRealFloat(_aiServo1AlarmCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo2AlarmCode", () => _GetRealFloat(_aiServo2AlarmCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo3AlarmCode", () => _GetRealFloat(_aiServo3AlarmCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo4AlarmCode", () => _GetRealFloat(_aiServo4AlarmCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.ServoSVRAlarmCode", () => _GetRealFloat(_aiServoSVRAlarmCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo1WarningCode", () => _GetRealFloat(_aiServo1WarningCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo2WarningCode", () => _GetRealFloat(_aiServo2WarningCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo3WarningCode", () => _GetRealFloat(_aiServo3WarningCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.Servo4WarningCode", () => _GetRealFloat(_aiServo4WarningCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.ServoSVRWarningCode", () => _GetRealFloat(_aiServoSVRWarningCode), SubscriptionAttribute.FLAG.IgnoreSaveDB);
            DATA.Subscribe($"{Module}.{Name}.ServoWorkMode", () => _GetRealFloat(_aoWorkMode), SubscriptionAttribute.FLAG.IgnoreSaveDB);

            OP.Subscribe($"{Module}.{Name}.SetState", (out string reason, int time, object[] param) => {

                reason = string.Empty;
                Position pos = Position.Invalid;
                if ((string)param[0] == "Up")
                    pos = Position.position1;
                else if ((string)param[0] == "Down")
                    pos = Position.position3;
                else if ((string)param[0] == "Middle")
                    pos = Position.position2;
                else
                {
                    reason = "Invalid moving position";
                    return false;
                }

                GoPosition(pos);

                return true;
            });

            OP.Subscribe($"{Module}.{Name}.Stop", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                ManulStop();
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.Up", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                ManulUp();
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.Down", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                ManulDown();
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.Home", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                GoPosition(Position.origin);
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.UpdateConfig", (out string reason, int time, object[] param) => {

                reason = string.Empty;

                return true;
            });

            OP.Subscribe($"{Module}.{Name}.StepDistance", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                SetRealFloat(_aoStepDistance, Convert.ToSingle(param[0]));
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.WorkMode", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                SetRealFloat(_aoWorkMode, Convert.ToSingle(param[0]));
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.SingleAxisManualSpeed", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                SetRealFloat(_aoSingleAxisManualSpeed, Convert.ToSingle(param[0]));
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.ServoJogUP", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                var servo = param[0].ToString();
                if (servo == "Servo1")
                {
                    ServoOP(_doServo1JogUP, _doServo1JogDOWN, _doServo1Stop);
                }
                else if (servo == "Servo2")
                {
                    ServoOP(_doServo2JogUP, _doServo2JogDOWN, _doServo2Stop);
                }
                else if (servo == "Servo3")
                {
                    ServoOP(_doServo3JogUP, _doServo3JogDOWN, _doServo3Stop);
                }
                else if (servo == "Servo4")
                {
                    ServoOP(_doServo4JogUP, _doServo4JogDOWN, _doServo4Stop);
                }

                return true;
            });

            OP.Subscribe($"{Module}.{Name}.ServoJogDOWN", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                var servo = param[0].ToString();
                if (servo == "Servo1")
                {
                    ServoOP(_doServo1JogDOWN, _doServo1JogUP, _doServo1Stop);
                }
                else if (servo == "Servo2")
                {
                    ServoOP(_doServo2JogDOWN, _doServo2JogUP, _doServo2Stop);
                }
                else if (servo == "Servo3")
                {
                    ServoOP(_doServo3JogDOWN, _doServo3JogUP, _doServo3Stop);
                }
                else if (servo == "Servo4")
                {
                    ServoOP(_doServo4JogDOWN, _doServo4JogUP, _doServo4Stop);
                }

                return true;
            });

            OP.Subscribe($"{Module}.{Name}.ServoStop", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                var servo = param[0].ToString();
                if (servo == "Servo1")
                {
                    ServoOP(_doServo1Stop, _doServo1JogUP, _doServo1JogDOWN, _doServo1StepUp, _doServo1StepDOWN);
                }
                else if (servo == "Servo2")
                {
                    ServoOP(_doServo2Stop, _doServo2JogUP, _doServo2JogDOWN, _doServo2StepUp, _doServo2StepDOWN);
                }
                else if (servo == "Servo3")
                {
                    ServoOP(_doServo3Stop, _doServo3JogUP, _doServo3JogDOWN, _doServo3StepUp, _doServo3StepDOWN);
                }
                else if (servo == "Servo4")
                {
                    ServoOP(_doServo4Stop, _doServo4JogUP, _doServo4JogDOWN, _doServo4StepUp, _doServo4StepDOWN);
                }
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.ServoStepUP", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                var servo = param[0].ToString();
                if (servo == "Servo1")
                {
                    ServoOP(_doServo1StepUp, _doServo1StepDOWN, _doServo1Stop);
                }
                else if (servo == "Servo2")
                {
                    ServoOP(_doServo2StepUp, _doServo2StepDOWN, _doServo2Stop);
                }
                else if (servo == "Servo3")
                {
                    ServoOP(_doServo3StepUp, _doServo3StepDOWN, _doServo3Stop);
                }
                else if (servo == "Servo4")
                {
                    ServoOP(_doServo4StepUp, _doServo4StepDOWN, _doServo4Stop);
                }
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.ServoStepDOWN", (out string reason, int time, object[] param) => {
                reason = string.Empty;
                var servo = param[0].ToString();
                if (servo == "Servo1")
                {
                    ServoOP(_doServo1StepDOWN, _doServo1StepUp, _doServo1Stop);
                }
                else if (servo == "Servo2")
                {
                    ServoOP(_doServo2StepDOWN, _doServo2StepUp, _doServo2Stop);
                }
                else if (servo == "Servo3")
                {
                    ServoOP(_doServo3StepDOWN, _doServo3StepUp, _doServo3Stop);
                }
                else if (servo == "Servo4")
                {
                    ServoOP(_doServo4StepDOWN, _doServo4StepUp, _doServo4Stop);
                }
                return true;
            });

            updatePinCfg();
            return true;
        }
        public bool SetRealFloat(IOAccessor<short> ao, float value)
        {
            if (_diABSAlarm.Value)
                return false;

            _SetRealFloat(ao, value);

            return true;
        }

        public void Terminate()
        {

        }
        public void Monitor()
        {
            if (_manualStopTimer.ElapsedMilliseconds > _stopButtonAutoResetTime)
            {
                _doStop.Value = false;
                _manualStopTimer.Stop();
            }

            if (_diORGAlarm.Value)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_diORGAlarm.Index} 回原点超时");
            }

            //if (_diServoAlarm.Value)
            //{
            //    NoDuplicatedAlarm($"Lift Pin DI-{_diServoAlarm.Index} 伺服驱动器报警");
            //}

            //if (_diCCWLimitSensorAlarm.Value)
            //{
            //    NoDuplicatedAlarm($"Lift Pin DI-{_diCCWLimitSensorAlarm.Index}  CCW Limit sensor 报警");
            //}

            if (_diOverSoftwareLimitAlarm.Value)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_diOverSoftwareLimitAlarm.Index}  Over Software Limit  报警");
            }

            if (_diABSAlarm.Value)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_diABSAlarm.Index}  ABS 报警");
            }

            if (_currentTarget == Position.Invalid)
                return;

            if ((_currentTarget == Position.position1 && _diP1.Value) ||
                (_currentTarget == Position.position2 && _diP2.Value) ||
                (_currentTarget == Position.position3 && _diP3.Value) ||
                (_currentTarget == Position.origin && _diOrigin.Value))
            {
                EV.PostInfoLog(Module, $"Lift Pin 到达 {_currentTarget}");
                Reset();
                return;
            }

            if (sw.ElapsedMilliseconds > _timeout)
            {
                NoDuplicatedAlarm($"Lift Pin 超时, 未到达 {_currentTarget}");
            }
        }

        public void Reset()
        {
            _currentTarget = Position.Invalid;
            sw.Reset();
            _doP1.Value = false;
            _doP2.Value = false;
            _doP3.Value = false;
            _doOrigin.Value = false;

            _doUp.Value = false;
            _doDown.Value = false;
            _doStop.Value = false;
            _bAlarmReported = false;

            EV.PostInfoLog(Module, $"Lift Pin reset all do to off.");
        }

        private void NoDuplicatedAlarm(string log)
        {
            if (_bAlarmReported == false)
            {
                EV.PostAlarmLog(Module, log);
                _bAlarmReported = true;
            }
        }
    }
}