Virgo_Mask/VirgoRT/Devices/IODevices/IoTriStateLift2.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.RT.Log;
using Aitex.Core.Util;

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

        private readonly DIAccessor _diCOMMAlarm;//去位置1，2，3超时
        private readonly DIAccessor _diBatteryLowAlarm;//去原点超时

        private readonly DIAccessor _di1_L_Servo_Driver_Alarm;
        private readonly DIAccessor _di1_L_Servo_Driver_Warning;
        private readonly DIAccessor _di1_Search_ORG_Fail_Alarm;
        private readonly DIAccessor _di1_ABS_Fail_Alarm;
        private readonly DIAccessor _di1_Up_Limit_Alarm;
        private readonly DIAccessor _di1_Down_Limit_Alarm;
        private readonly DIAccessor _di1_Over_Software_Alarm;

        private readonly DIAccessor _di2_L_Servo_Driver_Alarm;
        private readonly DIAccessor _di2_L_Servo_Driver_Warning;
        private readonly DIAccessor _di2_Search_ORG_Fail_Alarm;
        private readonly DIAccessor _di2_ABS_Fail_Alarm;
        private readonly DIAccessor _di2_Up_Limit_Alarm;
        private readonly DIAccessor _di2_Down_Limit_Alarm;
        private readonly DIAccessor _di2_Over_Software_Alarm;


        // 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 _aoServoWorkMode;
        private readonly AOAccessor _aoOriginSpeed;
        private readonly AOAccessor _aoAutoSpeed;
        private readonly AOAccessor _aoManualSpeed;
        private readonly AOAccessor _aoAccDecSpeedTime;
        private readonly AOAccessor _aoPosDiff;
        private readonly AOAccessor _aoDecTime;
        private readonly AOAccessor _aoSoftUpLimit;
        private readonly AOAccessor _aoSoftDownLimit;
        private readonly AOAccessor _aoCorrectionValue;

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

        private Position _currentTarget = Position.Invalid;
        private Position _preTarget = Position.Invalid;

        long _timeout = 60000;
        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 ((_preTarget == Position.position1 && _diP1.Value) && _diP2.Value == false && _diP3.Value == false)
                    return MovementPosition.Up;
                else if (_diP1.Value == false && (_diP2.Value && _preTarget == Position.position2) && _diP3.Value == false)
                    return MovementPosition.Middle;
                else if (_diP1.Value == false && _diP2.Value == false && (_diP3.Value && _preTarget == Position.position3))
                    return MovementPosition.Down;
                else if (_diOrigin.Value && _diOrigin2.Value && _diOrigin3.Value && _preTarget == Position.origin)
                    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 IoTriStateLift2(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);
            _diOrigin2 = ParseDiNode("diOrigin2", node, ioModule);
            _diOrigin3 = ParseDiNode("diOrigin3", node, ioModule);
            _diP1 = ParseDiNode("diP1", node, ioModule);
            _diP2 = ParseDiNode("diP2", node, ioModule);
            _diP3 = ParseDiNode("diP3", node, ioModule);

            _diCOMMAlarm = ParseDiNode("diCOMMAlarm", node, ioModule);
            _diBatteryLowAlarm = ParseDiNode("diBatteryLowAlarm", node, ioModule);

            _di1_L_Servo_Driver_Alarm = ParseDiNode("di1_L_Servo_Driver_Alarm", node, ioModule);
            _di1_L_Servo_Driver_Warning = ParseDiNode("di1_L_Servo_Driver_Warning", node, ioModule);
            _di1_Search_ORG_Fail_Alarm = ParseDiNode("di1_Search_ORG_Fail_Alarm", node, ioModule);
            _di1_ABS_Fail_Alarm = ParseDiNode("di1_ABS_Fail_Alarm", node, ioModule);
            _di1_Up_Limit_Alarm = ParseDiNode("di1_Up_Limit_Alarm", node, ioModule);
            _di1_Down_Limit_Alarm = ParseDiNode("di1_Down_Limit_Alarm", node, ioModule);
            _di1_Over_Software_Alarm = ParseDiNode("di1_Over_Software_Alarm", node, ioModule);

            _di2_L_Servo_Driver_Alarm = ParseDiNode("di2_L_Servo_Driver_Alarm", node, ioModule);
            _di2_L_Servo_Driver_Warning = ParseDiNode("di2_L_Servo_Driver_Warning", node, ioModule);
            _di2_Search_ORG_Fail_Alarm = ParseDiNode("di2_Search_ORG_Fail_Alarm", node, ioModule);
            _di2_ABS_Fail_Alarm = ParseDiNode("di2_ABS_Fail_Alarm", node, ioModule);
            _di2_Up_Limit_Alarm = ParseDiNode("di2_Up_Limit_Alarm", node, ioModule);
            _di2_Down_Limit_Alarm = ParseDiNode("di2_Down_Limit_Alarm", node, ioModule);
            _di2_Over_Software_Alarm = ParseDiNode("di2_Over_Software_Alarm", 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);
            _aoServoWorkMode = ParseAoNode("aoServoWorkMode", node, ioModule);
            _aoOriginSpeed = ParseAoNode("aoOriginSpeed", node, ioModule);
            _aoAutoSpeed = ParseAoNode("aoAutoSpeed", node, ioModule);
            _aoManualSpeed = ParseAoNode("aoManualSpeed", node, ioModule);
            _aoSoftUpLimit = ParseAoNode("aoSoftUpLimit", node, ioModule);
            _aoSoftDownLimit = ParseAoNode("aoSoftDownLimit", node, ioModule);
            _aoAccDecSpeedTime = ParseAoNode("aoAccDecSpeedTime", node, ioModule);
            _aoPosDiff = ParseAoNode("aoPosDiff", 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("ServoWorkMode", _aoServoWorkMode);
            _updateItem("OriginSpeed", _aoOriginSpeed);
            _updateItem("AutoSpeed", _aoAutoSpeed);
            _updateItem("ManualSpeed", _aoManualSpeed);
            _updateItem("SoftUpLimit", _aoSoftUpLimit);
            _updateItem("SoftDownLimit", _aoSoftDownLimit);
            _updateItem("AccDecSpeedTime", _aoAccDecSpeedTime);
            _updateItem("Position1", _aoSetP1);
            _updateItem("Position2", _aoSetP2);
            _updateItem("Position3", _aoSetP3);
            _updateItem("PosDiff", _aoPosDiff);

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

            _currentTarget = position;
            _preTarget = position;
            sw.Restart();
            swOrigin.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 && _diOrigin2.Value && _diOrigin3.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 (_diCOMMAlarm.Value)
                return false;
            _doUp.Value = false;
            _doDown.Value = false;
            _doStop.Value = true;
            _manualStopTimer.Restart();
            return true;
        }

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

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

            return true;
        }

        public bool ManulDown()
        {
            if (_diCOMMAlarm.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()
        {

        }
        private readonly R_TRIG _trigdiBatteryLowAlarm = new R_TRIG();
        private readonly R_TRIG _trigdiCOMMAlarm = new R_TRIG();
        private readonly R_TRIG _trigdi1_L_Servo_Driver_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi1_L_Servo_Driver_Warning = new R_TRIG();
        private readonly R_TRIG _trigdi1_Search_ORG_Fail_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi1_ABS_Fail_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi1_Up_Limit_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi1_Down_Limit_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi1_Over_Software_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi2_L_Servo_Driver_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi2_L_Servo_Driver_Warning = new R_TRIG();
        private readonly R_TRIG _trigdi2_Search_ORG_Fail_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi2_ABS_Fail_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi2_Up_Limit_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi2_Down_Limit_Alarm = new R_TRIG();
        private readonly R_TRIG _trigdi2_Over_Software_Alarm = new R_TRIG();
        public void Monitor()
        {
            if (_manualStopTimer.ElapsedMilliseconds > _stopButtonAutoResetTime)
            {
                _doStop.Value = false;
                _manualStopTimer.Stop();
            }

            _trigdiBatteryLowAlarm.CLK = _diBatteryLowAlarm.Value;
            if (_trigdiBatteryLowAlarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_diBatteryLowAlarm.Index} alarm");
            }

            _trigdiCOMMAlarm.CLK = _diCOMMAlarm.Value;
            if (_trigdiCOMMAlarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_diCOMMAlarm.Index}  alarm");
            }

            _trigdi1_L_Servo_Driver_Alarm.CLK = _di1_L_Servo_Driver_Alarm.Value;
            if (_trigdi1_L_Servo_Driver_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di1_L_Servo_Driver_Alarm.Index}  alarm");
            }
            _trigdi1_L_Servo_Driver_Warning.CLK = _di1_L_Servo_Driver_Warning.Value;
            if (_trigdi1_L_Servo_Driver_Warning.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di1_L_Servo_Driver_Warning.Index}  alarm");
            }
            _trigdi1_Search_ORG_Fail_Alarm.CLK = _di1_Search_ORG_Fail_Alarm.Value;
            if (_trigdi1_Search_ORG_Fail_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di1_Search_ORG_Fail_Alarm.Index}  alarm");
            }
            _trigdi1_ABS_Fail_Alarm.CLK = _di1_ABS_Fail_Alarm.Value;
            if (_trigdi1_ABS_Fail_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di1_ABS_Fail_Alarm.Index}  alarm");
            }
            _trigdi1_Up_Limit_Alarm.CLK = _di1_Up_Limit_Alarm.Value;
            if (_trigdi1_Up_Limit_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di1_Up_Limit_Alarm.Index}  alarm");
            }
            _trigdi1_Down_Limit_Alarm.CLK = _di1_Down_Limit_Alarm.Value;
            if (_trigdi1_Down_Limit_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di1_Down_Limit_Alarm.Index}  alarm");
            }
            _trigdi1_Over_Software_Alarm.CLK = _di1_Over_Software_Alarm.Value;
            if (_trigdi1_Over_Software_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di1_Over_Software_Alarm.Index}  alarm");
            }
            _trigdi2_L_Servo_Driver_Alarm.CLK = _di2_L_Servo_Driver_Alarm.Value;
            if (_trigdi2_L_Servo_Driver_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di2_L_Servo_Driver_Alarm.Index}  alarm");
            }
            _trigdi2_L_Servo_Driver_Warning.CLK = _di2_L_Servo_Driver_Warning.Value;
            if (_trigdi2_L_Servo_Driver_Warning.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di2_L_Servo_Driver_Warning.Index}  alarm");
            }
            _trigdi2_Search_ORG_Fail_Alarm.CLK = _di2_Search_ORG_Fail_Alarm.Value;
            if (_trigdi2_Search_ORG_Fail_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di2_Search_ORG_Fail_Alarm.Index}  alarm");
            }
            _trigdi2_ABS_Fail_Alarm.CLK = _di2_ABS_Fail_Alarm.Value;
            if (_trigdi2_ABS_Fail_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di2_ABS_Fail_Alarm.Index}  alarm");
            }
            _trigdi2_Up_Limit_Alarm.CLK = _di2_Up_Limit_Alarm.Value;
            if (_trigdi2_Up_Limit_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di2_Up_Limit_Alarm.Index}  alarm");
            }
            _trigdi2_Down_Limit_Alarm.CLK = _di2_Down_Limit_Alarm.Value;
            if (_trigdi2_Down_Limit_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di2_Down_Limit_Alarm.Index}  alarm");
            }

            _trigdi2_Over_Software_Alarm.CLK = _di2_Over_Software_Alarm.Value;
            if (_trigdi2_Over_Software_Alarm.Q)
            {
                NoDuplicatedAlarm($"Lift Pin DI-{_di2_Over_Software_Alarm.Index}  alarm");
            }

            if (_currentTarget == Position.Invalid)
                return;

            var servoOriginComplete = _diOrigin.Value;
            var servoOrigin2Complete = _diOrigin2.Value;
            var goPosOriginComplete = _diOrigin3.Value;
            if ((_currentTarget == Position.position1 && _diP1.Value) ||
                (_currentTarget == Position.position2 && _diP2.Value) ||
                (_currentTarget == Position.position3 && _diP3.Value) ||
                (_currentTarget == Position.origin && swOrigin.ElapsedMilliseconds > 1000 && (servoOriginComplete  && servoOrigin2Complete && goPosOriginComplete)))
            {
                //EV.PostInfoLog(Module, $"Lift Pin arrive {_currentTarget}, ({_diOrigin.Index})_DI_Lift_Servo_1_Servo_Origin_Complete={servoOriginComplete} ({_diOrigin2.Index})_DI_Lift_Servo_2_Servo_Origin_Complete={servoOrigin2Complete} ({_diOrigin3.Index})_DI_Lift_Servo_Group_Go_Pos_Origin_Complete={goPosOriginComplete} ");
                Reset();
                return;
            }

            if (sw.ElapsedMilliseconds > _timeout)
            {
                NoDuplicatedAlarm($"Lift Pin timeout, go {_currentTarget} failed");
            }
        }

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

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

            _trigdiBatteryLowAlarm.RST = true;
            _trigdiCOMMAlarm.RST = true;
            _trigdi1_L_Servo_Driver_Alarm.RST = true;
            _trigdi1_L_Servo_Driver_Warning.RST = true;
            _trigdi1_Search_ORG_Fail_Alarm.RST = true;
            _trigdi1_ABS_Fail_Alarm.RST = true;
            _trigdi1_Up_Limit_Alarm.RST = true;
            _trigdi1_Down_Limit_Alarm.RST = true;
            _trigdi1_Over_Software_Alarm.RST = true;
            _trigdi2_L_Servo_Driver_Alarm.RST = true;
            _trigdi2_L_Servo_Driver_Warning.RST = true;
            _trigdi2_Search_ORG_Fail_Alarm.RST = true;
            _trigdi2_ABS_Fail_Alarm.RST = true;
            _trigdi2_Up_Limit_Alarm.RST = true;
            _trigdi2_Down_Limit_Alarm.RST = true;
            _trigdi2_Over_Software_Alarm.RST = true;

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

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