FurnaceELK/Furnace/FurnaceRT/Devices/IoBufferMotor.cs

using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Device;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.IOCore;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Xml;

namespace FurnaceRT.Devices
{
    public class IoBufferMotor : BaseDevice, IDevice
    {
        #region fields
        protected enum State
        {
            Idle,
            Moving,
            ServoReset,
            Stopping,
        }
        protected State _state = State.Idle;
        protected DIAccessor _diServoOn;
        protected DIAccessor _diMoving;
        protected DIAccessor _diSensor1;
        protected DIAccessor _diSensor2;
        protected DIAccessor _diSensor3;
        protected DIAccessor _diSensor4;
        protected DIAccessor _diSensor5;
        protected DIAccessor _diSensor6;
        protected DIAccessor _diSensor7;
        protected DIAccessor _diSensor8;
        protected DIAccessor _diWarning;
        protected DIAccessor _diAlarm;
        protected DIAccessor _diNegativeLimit;
        protected DIAccessor _diPositiveLimit;
        protected DIAccessor _diPositionA1;
        protected DIAccessor _diPositionB1;
        protected DIAccessor _diPositionC1;
        protected DIAccessor _diPositionD1;
        protected DIAccessor _diPositionA2;
        protected DIAccessor _diPositionB2;
        protected DIAccessor _diPositionC2;
        protected DIAccessor _diPositionD2;
        protected DIAccessor _diPositionA3;
        protected DIAccessor _diPositionB3;
        protected DIAccessor _diPositionC3;
        protected DIAccessor _diPositionD3;
        protected DIAccessor _diPositionA4;
        protected DIAccessor _diPositionB4;
        protected DIAccessor _diPositionC4;
        protected DIAccessor _diPositionD4;

        protected DOAccessor _doServoOn;
        protected DOAccessor _doStop;
        protected DOAccessor _doMove;
        protected DOAccessor _doReset;

        protected AIAccessor _aiRealPosition;
        protected AIAccessor _aiRealSpeed;
        protected AIAccessor _aiRealTorque;
        protected AIAccessor _aiDriverErrorCode;
        protected AIAccessor _aiMotionErrorCode;
        protected AIAccessor _aiTargetPosFb;

        protected AOAccessor _aoTargetPosition;
        protected AOAccessor _aoSpeed;
        protected AOAccessor _aoAcc;
        protected AOAccessor _aoDec;
        protected SCConfigItem _scBufferRotationSpeed;
        protected SCConfigItem _scMoveTimeout;

        protected DeviceTimer _timer = new DeviceTimer();
        private const float PositionTolerance = 0.1f;
        private int setTime = 1500; //200ms set DO
        private int resetTime = 1200; //1200ms reset DO
        private int moveTimeout => (int)(_scMoveTimeout.IntValue * 1000);
        private PeriodicJob _thread;
        private R_TRIG _warningTrig = new R_TRIG();
        private R_TRIG _alarmTrig = new R_TRIG();
        public bool ServoOnOffSet
        {
            set
            {
                if (_doServoOn == null)
                    return;
                _doServoOn.Value = value;
            }
        }
        public float ServoMovePositionSet
        {
            set
            {
                if (_aoTargetPosition == null)
                    return;
                _aoTargetPosition.FloatValue = value;
            }
            get
            {
                return _aoTargetPosition.FloatValue;
            }
        }

        private bool _isFloatAioType = false;
        #endregion

        #region properties
        public float CurrentPosition => _aiRealPosition.FloatValue;
        public float CurrentSpeed => _aiRealSpeed.FloatValue;
        public bool IsError { get; private set; }
        public bool IsRunning => _diMoving == null ? false : _diMoving.Value;
        public bool IsReady
        {
            get
            {
                if (_diMoving != null && _diMoving.Value)
                    return false;

                return _state == State.Idle;
            }
        }
        public bool IsInPosition
        {
            get
            {
                if (_aoTargetPosition != null)
                {
                    switch (_aoTargetPosition.FloatValue)
                    {
                        case 1:
                            return (_diSensor1 != null ? _diSensor1.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionA1 != null ? _diPositionA1.Value : true);
                        case 9:
                            return (_diSensor1 != null ? _diSensor1.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionA3 != null ? _diPositionA3.Value : true);
                        case 2:
                            return (_diSensor2 != null ? _diSensor2.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionB1 != null ? _diPositionB1.Value : true);
                        case 10:
                            return (_diSensor2 != null ? _diSensor2.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionB3 != null ? _diPositionB3.Value : true);
                        case 3:
                            return (_diSensor3 != null ? _diSensor3.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionC1 != null ? _diPositionC1.Value : true);
                        case 11:
                            return (_diSensor3 != null ? _diSensor3.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionC3 != null ? _diPositionC3.Value : true);
                        case 4:
                            return (_diSensor4 != null ? _diSensor4.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionD1 != null ? _diPositionD1.Value : true);
                        case 12:
                            return (_diSensor4 != null ? _diSensor4.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionD3 != null ? _diPositionD3.Value : true);
                        case 5:
                            return (_diSensor5 != null ? _diSensor5.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionA2 != null ? _diPositionA2.Value : true);
                        case 13:
                            return (_diSensor5 != null ? _diSensor5.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionA4 != null ? _diPositionA4.Value : true);
                        case 6:
                            return (_diSensor6 != null ? _diSensor6.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionB2 != null ? _diPositionB2.Value : true);
                        case 14:
                            return (_diSensor6 != null ? _diSensor6.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionB4 != null ? _diPositionB4.Value : true);
                        case 7:
                            return (_diSensor7 != null ? _diSensor7.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionC2 != null ? _diPositionC2.Value : true);
                        case 15:
                            return (_diSensor7 != null ? _diSensor7.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionC4 != null ? _diPositionC4.Value : true);
                        case 8:
                            return (_diSensor8 != null ? _diSensor8.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionD2 != null ? _diPositionD2.Value : true);
                        case 16:
                            return (_diSensor8 != null ? _diSensor8.Value : true) && (_diMoving != null ? !_diMoving.Value : true) && (_diPositionD4 != null ? _diPositionD4.Value : true);
                    }
                }
                return false;
            }
        }
        public bool IsServoOn
        {
            get
            {
                if (_diServoOn == null)
                    return true;
                return _diServoOn.Value;
            }
        }
        public int TargetPositionFb => (int)(_aiTargetPosFb.FloatValue + 0.00001);
        public string ErrorCode => $"{(_aiDriverErrorCode != null ? ((int)(_aiDriverErrorCode.FloatValue + 0.00001)).ToString("X") : "")}/{(_aiMotionErrorCode != null ? ((int)(_aiMotionErrorCode.FloatValue + 0.00001)).ToString("X") : "")}";
        private R_TRIG _negativeLimitTrig = new R_TRIG();
        private R_TRIG _positiveLimitTrig = new R_TRIG();
        #endregion

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

            _isFloatAioType = !string.IsNullOrEmpty(node.GetAttribute("aioType")) && (node.GetAttribute("aioType") == "float");

            var scRootPath = string.IsNullOrEmpty(node.GetAttribute("scRootPath")) ? Module : node.GetAttribute("scRootPath");
            _diNegativeLimit = ParseDiNode("diNegativeLimit", node, ioModule);
            _diPositiveLimit = ParseDiNode("diPositiveLimit", node, ioModule);
            _diServoOn = ParseDiNode("diServoOn", node, ioModule);
            _diMoving = ParseDiNode("diMoving", node, ioModule);
            _diSensor1 = ParseDiNode("diSensor1", node, ioModule);
            _diSensor2 = ParseDiNode("diSensor2", node, ioModule);
            _diSensor3 = ParseDiNode("diSensor3", node, ioModule);
            _diSensor4 = ParseDiNode("diSensor4", node, ioModule);
            _diSensor5 = ParseDiNode("diSensor5", node, ioModule);
            _diSensor6 = ParseDiNode("diSensor6", node, ioModule);
            _diSensor7 = ParseDiNode("diSensor7", node, ioModule);
            _diSensor8 = ParseDiNode("diSensor8", node, ioModule);
            _diWarning = ParseDiNode("diWarning", node, ioModule);
            _diAlarm = ParseDiNode("diAlarm", node, ioModule);
            _diPositionA1 = ParseDiNode("diPositionA1", node, ioModule);
            _diPositionB1 = ParseDiNode("diPositionB1", node, ioModule);
            _diPositionC1 = ParseDiNode("diPositionC1", node, ioModule);
            _diPositionD1 = ParseDiNode("diPositionD1", node, ioModule);
            _diPositionA2 = ParseDiNode("diPositionA2", node, ioModule);
            _diPositionB2 = ParseDiNode("diPositionB2", node, ioModule);
            _diPositionC2 = ParseDiNode("diPositionC2", node, ioModule);
            _diPositionD2 = ParseDiNode("diPositionD2", node, ioModule);
            _diPositionA3 = ParseDiNode("diPositionA3", node, ioModule);
            _diPositionB3 = ParseDiNode("diPositionB3", node, ioModule);
            _diPositionC3 = ParseDiNode("diPositionC3", node, ioModule);
            _diPositionD3 = ParseDiNode("diPositionD3", node, ioModule);
            _diPositionA4 = ParseDiNode("diPositionA4", node, ioModule);
            _diPositionB4 = ParseDiNode("diPositionB4", node, ioModule);
            _diPositionC4 = ParseDiNode("diPositionC4", node, ioModule);
            _diPositionD4 = ParseDiNode("diPositionD4", node, ioModule);

            _doServoOn = ParseDoNode("doServoOn", node, ioModule);
            _doStop = ParseDoNode("doStop", node, ioModule);
            _doMove = ParseDoNode("doMove", node, ioModule);
            _doReset = ParseDoNode("doReset", node, ioModule);

            _aiRealPosition = ParseAiNode("aiRealPosition", node, ioModule);
            _aiRealSpeed = ParseAiNode("aiRealSpeed", node, ioModule);
            _aiDriverErrorCode = ParseAiNode("aiDriverErrorCode", node, ioModule);
            _aiMotionErrorCode = ParseAiNode("aiMotionErrorCode", node, ioModule);
            _aiTargetPosFb = ParseAiNode("aiTargetPosFb", node, ioModule);
            _aiRealTorque = ParseAiNode("aiRealTorque", node, ioModule);

            _aoTargetPosition = ParseAoNode("aoTargetPosition", node, ioModule);
            _aoSpeed = ParseAoNode("aoSpeed", node, ioModule);
            _aoAcc = ParseAoNode("aoAcc", node, ioModule);
            _aoDec = ParseAoNode("aoDec", node, ioModule);

            _scMoveTimeout = ParseScNode("scMoveTimeout", node, ioModule, $"{scRootPath}.MotionTimeout");
            _scBufferRotationSpeed = ParseScNode("scBufferRotationSpeed", node, ioModule, $"{scRootPath}.MoveSpeed");

            if (_doServoOn != null)
                _doServoOn.Value = true;

            _thread = new PeriodicJob(50, OnTimer, Name);
            _thread.Start();
        }
        private void InitSCAOData()
        {

            if (_scBufferRotationSpeed != null && _aoSpeed != null)
                _aoSpeed.FloatValue = (float)_scBufferRotationSpeed.DoubleValue;


        }
        public virtual bool Initialize()
        {
            DATA.Subscribe($"{Module}.{Name}.CurrentPosition", () => _aiRealPosition != null ? (_isFloatAioType ? _aiRealPosition.FloatValue : _aiRealPosition.Value) : 0);
            DATA.Subscribe($"{Module}.{Name}.CurrentSpeed", () => _aiRealSpeed != null ? (_isFloatAioType ? _aiRealSpeed.FloatValue : _aiRealSpeed.Value) : 0);
            DATA.Subscribe($"{Module}.{Name}.CurrentTorque", () => _aiRealTorque != null ? (_isFloatAioType ? _aiRealTorque.FloatValue : _aiRealTorque.Value) : 0);
            DATA.Subscribe($"{Module}.{Name}.TargetPosition", () => (int)(_aoTargetPosition.Value + 0.00001));
            DATA.Subscribe($"{Module}.{Name}.IsInPosition", () => IsInPosition);
            DATA.Subscribe($"{Module}.{Name}.IsServoOn", () => IsServoOn);
            DATA.Subscribe($"{Module}.{Name}.IsReady", () => IsReady);
            DATA.Subscribe($"{Module}.{Name}.IsAlarm", () => _diAlarm != null ? _diAlarm.Value : false);
            DATA.Subscribe($"{Module}.{Name}.IsWarning", () => _diWarning != null ? _diWarning.Value : false);
            DATA.Subscribe($"{Module}.{Name}.Status", () => _state.ToString());
            DATA.Subscribe($"{Module}.{Name}.IsMotorRun", () => _diMoving != null ? _diMoving.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionA1", () => _diPositionA1 != null ? _diPositionA1.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionB1", () => _diPositionB1 != null ? _diPositionB1.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionC1", () => _diPositionC1 != null ? _diPositionC1.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionD1", () => _diPositionD1 != null ? _diPositionD1.Value : false);

            DATA.Subscribe($"{Module}.{Name}.AtPositionA2", () => _diPositionA2 != null ? _diPositionA2.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionB2", () => _diPositionB2 != null ? _diPositionB2.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionC2", () => _diPositionC2 != null ? _diPositionC2.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionD2", () => _diPositionD2 != null ? _diPositionD2.Value : false);

            DATA.Subscribe($"{Module}.{Name}.AtPositionA3", () => _diPositionA3 != null ? _diPositionA3.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionB3", () => _diPositionB3 != null ? _diPositionB3.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionC3", () => _diPositionC3 != null ? _diPositionC3.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionD3", () => _diPositionD3 != null ? _diPositionD3.Value : false);

            DATA.Subscribe($"{Module}.{Name}.AtPositionA4", () => _diPositionA4 != null ? _diPositionA4.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionB4", () => _diPositionB4 != null ? _diPositionB4.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionC4", () => _diPositionC4 != null ? _diPositionC4.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPositionD4", () => _diPositionD4 != null ? _diPositionD4.Value : false);
            OP.Subscribe($"{Module}.{Name}.ServoTargetPosition", (string cmd, object[] param) =>
            {
                float.TryParse(param[0].ToString(), out float position);
                SetServoTargetPosition(position);
                return true;
            });
            OP.Subscribe($"{Module}.{Name}.ServoMoveTo", (string cmd, object[] param) =>
            {
                if (!IsServoOn)
                {
                    EV.PostWarningLog($"{Module}", $"{Name} servo not on");
                    return false;
                }
                if (_state != State.Idle)
                {
                    EV.PostWarningLog($"{Module}", $"{Name} busy, wait");
                    return false;
                }
                SetServoMoveTo();
                return true;
            });
            OP.Subscribe($"{Module}.{Name}.ServoOnOff", (string cmd, object[] param) =>
            {
                bool.TryParse(param[0].ToString(), out bool isOn);
                SetServoOnOff(isOn);
                return true;
            });
            OP.Subscribe($"{Module}.{Name}.ServoResetAlarm", (string cmd, object[] param) =>
            {
                if (_state != State.Idle)
                {
                    EV.PostWarningLog($"{Module}", $"{Name} busy, wait");
                    return false;
                }
                ServoReset(out _);
                return true;
            });
            OP.Subscribe($"{Module}.{Name}.ServoStop", (string cmd, object[] param) =>
            {
                ServoStop();
                return true;
            });
            InitSCAOData();
            return true;
        }

        public virtual void Monitor()
        {
            if (_diNegativeLimit != null)
            {
                _negativeLimitTrig.CLK = _diNegativeLimit.Value;
                if (_negativeLimitTrig.Q)
                    EV.PostWarningLog(Module, $"{Module} {Name} at negative limit position");
            }
            if (_diPositiveLimit != null)
            {
                _positiveLimitTrig.CLK = _diPositiveLimit.Value;
                if (_positiveLimitTrig.Q)
                    EV.PostWarningLog(Module, $"{Module} {Name} at positive limit position");
            }
            if (_diWarning != null)
            {
                _warningTrig.CLK = _diWarning.Value;
                if (_warningTrig.Q)
                    EV.PostWarningLog(Module, $"{Module} {Name} warning code={ErrorCode}");
            }
            if (_diAlarm != null)
            {
                _alarmTrig.CLK = _diAlarm.Value;
                if (_alarmTrig.Q)
                    EV.PostWarningLog(Module, $"{Module} {Name} alarm code={ErrorCode}");
            }
        }

        public virtual void Reset()
        {
            ServoReset(out _);
        }

        public virtual void Terminate()
        {

        }

        private bool OnTimer()
        {
            switch (_state)
            {
                case State.Idle:
                    ResetDO();
                    _timer.Stop();
                    break;

                case State.Moving:
                    if (_timer.GetElapseTime() >= setTime)
                    {
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _doMove.SetValue(false, out _);
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Name}", $"{Name} Servo move timeout");
                            IsError = true;
                        }
                        else
                        {
                            if (IsInPosition)
                            {
                                _doMove.SetValue(false, out _);
                                _state = State.Idle;
                                IsError = false;
                            }
                        }
                    }
                    break;
                case State.Stopping:
                    if (_timer.GetElapseTime() >= setTime)
                    {
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _doStop.SetValue(false, out _);
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Name}", $"{Name} Servo stop timeout");
                            IsError = true;
                        }
                        else
                        {
                            if (!_diMoving.Value)
                            {
                                _doStop.SetValue(false, out _);
                                _state = State.Idle;
                                IsError = false;
                            }
                        }
                    }
                    break;
            }
            return true;
        }
        public void SetServoOnOff(bool isOn)
        {
            ServoOnOffSet = isOn;
        }
        public void SetServoTargetPosition(float position)
        {
            ServoMovePositionSet = position;
            InitSCAOData();
        }
        public bool SetServoMoveTo()
        {
            ResetDO();

            _state = State.Moving;
            _doMove.SetValue(true, out _);
            _timer.Start(0);
            return true;
        }
        public bool ServoReset(out string reason)
        {
            reason = string.Empty;
            ResetDO();
            _doReset.SetPulseValue(true, 1000);

            return true;
        }
        public void ServoStop()
        {
            ResetDO();
            _doStop.SetPulseValue(true, 1000);
            _doMove.SetValue(false, out _);
            _state = State.Idle;
        }
        protected void ResetDO()
        {
            if (_doMove != null && !_doMove.Value)
                _doMove.SetValue(false, out _);
            if (_doStop != null && !_doStop.Value)
                _doStop.SetValue(false, out _);
            if (_doReset != null && !_doReset.Value)
                _doReset.SetValue(false, out _);
        }
    }
}