Virgo_Mask/VirgoRT/Devices/IODevices/IoTriStateLift.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;

namespace VirgoRT.Devices
{
    public enum Position
    {
        position1,
        position2,
        position3,
        origin,
        Invalid,
    }
    public class IoTriStateLift : 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 AIAccessor _currentValue;

        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 _aoAccTime;
        private readonly AOAccessor _aoDecTime;
        private readonly AOAccessor _aoCCWLimitPosition;
        private readonly AOAccessor _aoCWLimitPosition;
        private readonly AOAccessor _aoCorrectionValue;

        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 IoTriStateLift(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);
            _diServoAlarm = ParseDiNode("diServoAlarm", node, ioModule);
            _diCCWLimitSensorAlarm = ParseDiNode("diCCWLimitAlarm", node, ioModule);
            _diOverSoftwareLimitAlarm = ParseDiNode("diOverSoftwareLimitAlarm", 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);

            _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);
            _aoAccTime          = ParseAoNode("aoAccTime", node, ioModule);
            _aoDecTime          = ParseAoNode("aoDecTime", node, ioModule);
            _aoCCWLimitPosition = ParseAoNode("aoCCWLimitPosition", node, ioModule);
            _aoCWLimitPosition  = ParseAoNode("aoCWLimitPosition", node, ioModule);
            _aoCorrectionValue  = ParseAoNode("aoCorrectionValue", node, ioModule);
        }

        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("AccTime", _aoAccTime);
            _updateItem("DecTime", _aoDecTime);
            _updateItem("CCWLimitPosition", _aoCCWLimitPosition);
            _updateItem("CWLimitPosition", _aoCWLimitPosition);
            _updateItem("CorrectionValue", _aoCorrectionValue);
            _updateItem("Position1", _aoSetP1);
            _updateItem("Position2", _aoSetP2);
            _updateItem("Position3", _aoSetP3);
             
        }
        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 float CurrentValue
        {
            get
            {
                if (_currentValue == null)
                {
                    return 0;
                }

                return _GetRealFloat(_currentValue); ;
            }
        }

        public bool Initialize()
        {

            DATA.Subscribe($"{Module}.{Name}.DeviceData", () => DeviceData);

            DATA.Subscribe($"{Module}.{Name}.PinPosition", () => PinPositionint);

            DATA.Subscribe($"{Module}.{Name}.CurrentValue", () => CurrentValue);

            DATA.Subscribe($"{Module}.{Name}.ManualStopState", () => _doStop.Value);
            DATA.Subscribe($"{Module}.{Name}.ManualUpState", () => _doUp.Value);
            DATA.Subscribe($"{Module}.{Name}.ManualDownState", () => _doDown.Value) ;

            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;
            });

            updatePinCfg();
            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} ORG alarm");
            }

            if (_diServoAlarm.Value)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_diServoAlarm.Index} servo alarm");
            }

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

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

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

            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 arrive {_currentTarget}");
                Reset();
                return;
            }

            if (sw.ElapsedMilliseconds > _timeout)
            {
                NoDuplicatedAlarm($"Lift Pin timeout, Not arrived yet {_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;
            }
        }
    }
}