FurnaceELK/Furnace/FurnaceRT/Devices/IoFurnaceMotor.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 DocumentFormat.OpenXml.Spreadsheet;
using FurnaceRT.Equipments.Boats;
using FurnaceRT.Equipments.Systems;
using MECF.Framework.Common.Equipment;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Xml;

namespace FurnaceRT.Devices
{
    public class IoFurnaceMotor : BaseDevice, IDevice
    {
        #region fields
        protected enum State
        {
            Idle,
            MoveToPosition,
            Rotating,
            Homing,
            Jogging,
            Pitching,
            Stopping,
            ServoOff,
            ServoOn,
            ServoReset,
        }
        public enum Direction
        {
            Unknown,
            CW,
            CCW,
        }
        protected State _state = State.Idle;
        protected DIAccessor _diHoming;
        protected DIAccessor _diInitializing;
        protected DIAccessor _diHomeDone;
        protected DIAccessor _diInitDone;
        protected DIAccessor _diMoving;
        protected DIAccessor _diMotorWarning;
        protected DIAccessor _diMotorAlarm;
        protected DIAccessor _diAlarm;
        protected DIAccessor _diServoOn;
        protected DIAccessor _diDirection;

        protected DIAccessor _diM0;
        protected DIAccessor _diM1;
        protected DIAccessor _diM2;
        protected DIAccessor _diM3;
        protected DIAccessor _diM4;
        protected DIAccessor _diM5;

        protected DIAccessor _diHomePosition;
        protected DIAccessor _diNegativeLimit;
        protected DIAccessor _diPositiveLimit;
        protected DIAccessor _diPosition1;
        protected DIAccessor _diPosition2;
        protected DIAccessor _diPosition3;

        protected DOAccessor _doStop;
        protected DOAccessor _doHome;
        protected DOAccessor _doInit;
        protected DOAccessor _doMove;
        protected DOAccessor _doReset;
        protected DOAccessor _doServoOn;
        protected DOAccessor _doCW;
        protected DOAccessor _doCCW;
        protected DOAccessor _doM0;
        protected DOAccessor _doM1;
        protected DOAccessor _doM2;
        protected DOAccessor _doM3;
        protected DOAccessor _doM4;
        protected DOAccessor _doM5;

        protected AIAccessor _aiRealPosition;
        protected AIAccessor _aiRealSpeed;
        protected AIAccessor _aiBoatElevatorErrorCode;
        protected AIAccessor _aiTargetSpeed;
        protected AIAccessor _aiDriverErrorCode;
        protected AIAccessor _aiMotionErrorCode;
        protected AIAccessor _aiTargetPosFb;

        protected AOAccessor _aoTargetPosition;
        protected AOAccessor _aoTargetSpeed;
        protected AOAccessor _aoAcc;
        protected AOAccessor _aoDec;

        protected SCConfigItem _scServoMoveSpeed;
        protected SCConfigItem _scServoAcc;
        protected SCConfigItem _scServoDec;
        protected SCConfigItem _scServoPosition1;
        protected SCConfigItem _scServoPosition2;
        protected SCConfigItem _scServoPosition3;
        protected SCConfigItem _scServoPosition4;
        protected SCConfigItem _scServoPosition5;
        protected SCConfigItem _scServoPosition6;
        protected SCConfigItem _scServoPosition7;
        protected SCConfigItem _scServoPosition8;
        protected SCConfigItem _scServoPosition9;
        protected SCConfigItem _scServoPosition10;
        protected SCConfigItem _scMoveTimeout;

        protected DeviceTimer _timer = new DeviceTimer();
        private const float PositionTolerance = 0.1f;
        private int setTime = 100; //200ms set DO
        private int resetTime = 2000; //2000 reset DO
        private int moveTimeout => (int)(_scMoveTimeout.IntValue * 1000);
        private PeriodicJob _thread;
        private R_TRIG _stateTrig = new R_TRIG();
        private R_TRIG _negativeLimitTrig = new R_TRIG();
        private R_TRIG _positiveLimitTrig = new R_TRIG();
        private R_TRIG _warningTrig = new R_TRIG();
        private R_TRIG _alarmTrig = new R_TRIG();
        private string _lastTarget = "";
        private bool IsRAxis = false;
        public bool ServoOnOffSet
        {
            set
            {
                if (_doServoOn == null)
                    return;
                _doServoOn.Value = value;
            }
        }
        public float ServoMoveSpeedSet
        {
            set
            {
                if (_aoTargetSpeed == null)
                    return;
                _aoTargetSpeed.FloatValue = value;
            }
        }
        public float ServoAccSet
        {
            set
            {
                if (_aoAcc == null)
                    return;
                _aoAcc.FloatValue = value;
            }
        }
        public float ServoDecSet
        {
            set
            {
                if (_aoDec == null)
                    return;
                _aoDec.FloatValue = value;
            }
        }
        public float ServoMovePositionSet
        {
            set
            {
                if (_aoTargetPosition == null)
                    return;
                _aoTargetPosition.FloatValue = value;
            }
            get
            {
                return _aoTargetPosition.FloatValue;
            }
        }

        public Direction MotorDirection
        {
            get
            {
                if (_diDirection != null)
                    return _diDirection.Value ? Direction.CCW : Direction.CW;

                if (_doCW != null && _doCCW != null)
                {
                    if (_doCW.Value)
                        return Direction.CW;
                    if (_doCCW.Value)
                        return Direction.CCW;
                }

                return Direction.Unknown;
            }
        }
        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 bool _isFloatAioType = false;
        public bool IsHomingState = false;
        #endregion

        #region properties
        public float CurrentPosition => _aiRealPosition.FloatValue;
        public float CurrentSpeed => _aiRealSpeed.FloatValue;
        public Dictionary<string, string> IoMappingDic { set; get; }
        public bool IsError
        {
            get
            {
                if (_diMotorAlarm != null && _diMotorAlarm.Value)
                    return true;

                if (_diAlarm != null && _diAlarm.Value)
                    return true;

                return false;
            }
        }
        public bool IsHomeing => _diHoming == null ? false : _diHoming.Value;
        public bool IsHomeDone => _diHomeDone == null ? false : _diHomeDone.Value;
        public bool IsInitDone => _diInitDone == null ? false : _diInitDone.Value;
        public bool IsMoving => _diMoving == null ? false : _diMoving.Value;
        public bool IsPause { get; set; }
        public bool IsReady
        {
            get
            {
                if (_diMotorWarning != null && _diMotorWarning.Value)
                    return false;
                if (_diMotorAlarm != null && _diMotorAlarm.Value)
                    return false;
                if (_diAlarm != null && _diAlarm.Value)
                    return false;
                if (_diServoOn != null && !_diServoOn.Value)
                    return false;
                if (_diInitializing != null && _diInitializing.Value)
                    return false;
                if (_diHoming != null && _diHoming.Value)
                    return false;
                if (_diMoving != null && _diMoving.Value)
                    return false;

                return _state == State.Idle;
            }
        }
        public bool IsServoOn
        {
            get
            {
                if (_diServoOn == null)
                    return true;
                return _diServoOn.Value;
            }
        }
        private R_TRIG _homingTrig = new R_TRIG();
        #endregion

        public IoFurnaceMotor(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");

            _diHoming = ParseDiNode("diHoming", node, ioModule);
            _diInitializing = ParseDiNode("diInitializing", node, ioModule);
            _diHomeDone = ParseDiNode("diHomeDone", node, ioModule);
            _diInitDone = ParseDiNode("diInitDone", node, ioModule);
            _diMoving = ParseDiNode("diMoving", node, ioModule);
            _diMotorWarning = ParseDiNode("diMotorWarning", node, ioModule);
            _diMotorAlarm = ParseDiNode("diMotorAlarm", node, ioModule);
            _diAlarm = ParseDiNode("diAlarm", node, ioModule);
            _diServoOn = ParseDiNode("diServoOn", node, ioModule);
            _diDirection = ParseDiNode("diDirection", node, ioModule);

            _diM0 = ParseDiNode("diM0", node, ioModule);
            _diM1 = ParseDiNode("diM1", node, ioModule);
            _diM2 = ParseDiNode("diM2", node, ioModule);
            _diM3 = ParseDiNode("diM3", node, ioModule);
            _diM4 = ParseDiNode("diM4", node, ioModule);
            _diM5 = ParseDiNode("diM5", node, ioModule);

            _diHomePosition = ParseDiNode("diHomePosition", node, ioModule);
            _diNegativeLimit = ParseDiNode("diNegativeLimit", node, ioModule);
            _diPositiveLimit = ParseDiNode("diPositiveLimit", node, ioModule);
            _diPosition1 = ParseDiNode("diPosition1", node, ioModule);
            _diPosition2 = ParseDiNode("diPosition2", node, ioModule);
            _diPosition3 = ParseDiNode("diPosition3", node, ioModule);

            _doStop = ParseDoNode("doStop", node, ioModule);
            _doHome = ParseDoNode("doHome", node, ioModule);
            _doInit = ParseDoNode("doInit", node, ioModule);
            _doMove = ParseDoNode("doMove", node, ioModule);
            _doReset = ParseDoNode("doReset", node, ioModule);
            _doServoOn = ParseDoNode("doServoOn", node, ioModule);
            _doCW = ParseDoNode("doCW", node, ioModule);
            _doCCW = ParseDoNode("doCCW", node, ioModule);
            _doM0 = ParseDoNode("doM0", node, ioModule);
            _doM1 = ParseDoNode("doM1", node, ioModule);
            _doM2 = ParseDoNode("doM2", node, ioModule);
            _doM3 = ParseDoNode("doM3", node, ioModule);
            _doM4 = ParseDoNode("doM4", node, ioModule);
            _doM5 = ParseDoNode("doM5", node, ioModule);

            _aiRealPosition = ParseAiNode("aiRealPosition", node, ioModule);
            _aiRealSpeed = ParseAiNode("aiRealSpeed", node, ioModule);
            _aiBoatElevatorErrorCode = ParseAiNode("aiBoatElevatorErrorCode", node, ioModule);
            _aiTargetSpeed = ParseAiNode("aiTargetSpeed", node, ioModule);
            _aiDriverErrorCode = ParseAiNode("aiDriverErrorCode", node, ioModule);
            _aiMotionErrorCode = ParseAiNode("aiMotionErrorCode", node, ioModule);
            _aiTargetPosFb = ParseAiNode("aiTargetPosFb", node, ioModule);

            _aoTargetPosition = ParseAoNode("aoTargetPosition", node, ioModule);
            _aoTargetSpeed = ParseAoNode("aoTargetSpeed", node, ioModule);
            _aoAcc = ParseAoNode("aoAcc", node, ioModule);
            _aoDec = ParseAoNode("aoDec", node, ioModule);

            _scServoMoveSpeed = ParseScNode("scSpeed1", node, ioModule, $"{scRootPath}.MoveSpeed");
            _scServoAcc = ParseScNode("scSpeed1", node, ioModule, $"{scRootPath}.Acc");
            _scServoDec = ParseScNode("scSpeed1", node, ioModule, $"{scRootPath}.Dec");
            _scServoPosition1 = ParseScNode("scServoPosition1", node, ioModule, $"{scRootPath}.Position1");
            _scServoPosition2 = ParseScNode("scServoPosition2", node, ioModule, $"{scRootPath}.Position2");
            _scServoPosition3 = ParseScNode("scServoPosition3", node, ioModule, $"{scRootPath}.Position3");
            _scServoPosition4 = ParseScNode("scServoPosition4", node, ioModule, $"{scRootPath}.Position4");
            _scServoPosition5 = ParseScNode("scServoPosition5", node, ioModule, $"{scRootPath}.Position5");
            _scServoPosition6 = ParseScNode("scServoPosition6", node, ioModule, $"{scRootPath}.Position6");
            _scServoPosition7 = ParseScNode("scServoPosition7", node, ioModule, $"{scRootPath}.Position7");
            _scServoPosition8 = ParseScNode("scServoPosition8", node, ioModule, $"{scRootPath}.Position8");
            _scServoPosition9 = ParseScNode("scServoPosition9", node, ioModule, $"{scRootPath}.Position9");
            _scServoPosition10 = ParseScNode("scServoPosition10", node, ioModule, $"{scRootPath}.Position10");
            _scMoveTimeout = ParseScNode("scMoveTimeout", node, ioModule, $"{scRootPath}.MotionTimeout");
            IsRAxis = Name == "BoatRotationServo";
            _thread = new PeriodicJob(50, OnTimer, Name);
            _thread.Start();
        }

        public virtual bool Initialize()
        {
            DATA.Subscribe($"{Module}.{Name}.CurrentPosition", () => _aiRealPosition != null ? (_isFloatAioType ? _aiRealPosition.FloatValue : _aiRealPosition.Value) : 0);
            DATA.Subscribe($"{Module}.{Name}.CurrentSpeed", () => GetAxisSpeed());
            if (_aiTargetPosFb != null)
                DATA.Subscribe($"{Module}.{Name}.TargetPositionFb", () => _aiTargetPosFb != null ? (_isFloatAioType ? _aiTargetPosFb.FloatValue : _aiTargetPosFb.Value) : 0);
            DATA.Subscribe($"{Module}.{Name}.TargetPosition", () => _aoTargetPosition != null ? (_isFloatAioType ? _aoTargetPosition.FloatValue : _aoTargetPosition.Value) : 0);
            DATA.Subscribe($"{Module}.{Name}.IsReady", () => IsReady);
            DATA.Subscribe($"{Module}.{Name}.IsWarning", () => _diMotorWarning != null ? _diMotorWarning.Value : false);
            DATA.Subscribe($"{Module}.{Name}.IsAlarm", () => IsError);
            DATA.Subscribe($"{Module}.{Name}.IsServoOn", () => _diServoOn != null ? _diServoOn.Value : false);
            DATA.Subscribe($"{Module}.{Name}.ErrorCode", () => ErrorCode);
            DATA.Subscribe($"{Module}.{Name}.Status", () => _state.ToString());
            DATA.Subscribe($"{Module}.{Name}.Direction", () => MotorDirection.ToString());
            DATA.Subscribe($"{Module}.{Name}.IsMoving", () => _diMoving != null ? _diMoving.Value : false);
            DATA.Subscribe($"{Module}.{Name}.IsInitDone", () => _diInitDone != null ? _diInitDone.Value : false);
            DATA.Subscribe($"{Module}.{Name}.IsHomeDone", () => _diHomeDone != null ? _diHomeDone.Value : false);
            DATA.Subscribe($"{Module}.{Name}.IsHoming", () => _diHoming != null ? _diHoming.Value : false);
            DATA.Subscribe($"{Module}.{Name}.IsInitializing", () => _diInitializing != null ? _diInitializing.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPosition1", () => _diPosition1 != null ? _diPosition1.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPosition2", () => _diPosition2 != null ? _diPosition2.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtPosition3", () => _diPosition3 != null ? _diPosition3.Value : false);
            DATA.Subscribe($"{Module}.{Name}.AtHomePosition", () => _diHomePosition != null ? _diHomePosition.Value : false);
            OP.Subscribe($"{Module}.{Name}.SetServoPara", (string cmd, object[] param) =>
            {
                float.TryParse(param[1].ToString(), out float value);
                SetServoPara(param[0].ToString(), value);
                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;
                }
                float setSpeed = 0.0f;
                if (param.Length >= 2)
                {
                    float.TryParse(param[1].ToString(), out setSpeed);
                }
                SetServoMoveTo(param[0].ToString(), out _, setSpeed);
                return true;
            });
            OP.Subscribe($"{Module}.{Name}.ServoHome", (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;
                }
                SetServoHome();
                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) =>
            {
                ServoReset(out _);
                return true;
            });
            OP.Subscribe($"{Module}.{Name}.ServoStop", (string cmd, object[] param) =>
            {
                ServoStop();
                return true;
            });
            OP.Subscribe($"{Module}.{Name}.Continue", (string cmd, object[] param) =>
            {
                Continue();
                return true;
            });
            if (_scServoMoveSpeed != null)
                SetServoMoveSpeed((float)_scServoMoveSpeed.DoubleValue);

            if (_scServoAcc != null)
                ServoAccSet = (float)_scServoAcc.DoubleValue;

            if (_scServoDec != null)
                ServoDecSet = (float)_scServoDec.DoubleValue;

            if (_doServoOn != null)
                _doServoOn.SetValue(true, out _);
            return true;
        }
        public float GetSpeedFromOutputs()
        {
            if (_diMoving.Value || _diHoming.Value)
            {
                return GetRAxisSpeed();
            }
            return 0f;
        }
        private float GetRAxisSpeed()
        {

            List<bool> chunks = new List<bool> { _diM0.Value, _diM1.Value, _diM2.Value, _diM3.Value, _diM4.Value, _diM5.Value };
            chunks.Reverse();

            string binaryString = "";
            foreach (bool chunk in chunks)
            {
                binaryString += chunk ? "1" : "0";
            }

            int intValue = Convert.ToInt32(binaryString, 2);

            if (intValue >= 8 && intValue <= 47)
            {
                decimal standardSpeed = 0.1M;
                for (int i = 8; i < intValue; i++)
                {
                    standardSpeed += 0.1M;
                }
                return (float)standardSpeed;
            }
            else
            {
                return 0f;
            }
        }
        private float GetAxisSpeed()
        {
            if (IsRAxis)
            {
                return _aiRealSpeed != null ? (_isFloatAioType ? _aiRealSpeed.FloatValue : _aiRealSpeed.Value) : GetSpeedFromOutputs();

            }
            else
            {
                return _aiRealSpeed != null ? (_isFloatAioType ? _aiRealSpeed.FloatValue : _aiRealSpeed.Value) : 0;


            }

        }
        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 (_diMotorWarning != null)
            {
                _warningTrig.CLK = _diMotorWarning.Value;
                if (_warningTrig.Q)
                    EV.PostWarningLog(Module, $"{Module} {Name} warning code={ErrorCode}");
            }
            if (IsError)
            {
                _alarmTrig.CLK = IsError;
                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:
                    if (!_timer.IsIdle())
                    {
                        _timer.Stop();
                    }
                    _stateTrig.CLK = true;
                    if (_stateTrig.Q)
                        ResetDO();
                    break;

                case State.Homing:
                    if (_timer.GetElapseTime() >= setTime)
                    {
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _doHome.SetValue(false, out _);
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Module}", $"{Name} Motor home timeout");
                        }
                        else
                        {
                            _homingTrig.CLK = !_diHoming.Value;
                            if (_homingTrig.Q)
                            {
                                if (_diHomeDone != null && _diHomeDone.Value)
                                {
                                    _doHome.SetValue(false, out _);
                                    _state = State.Idle;
                                    IsHomingState = false;
                                    EV.PostInfoLog($"{Module}", $"{Name} Motor home finish");
                                }
                            }
                        }
                    }
                    if (_timer.GetElapseTime() >= resetTime)
                    {
                        _doHome.SetValue(false, out _);
                    }
                    break;

                case State.ServoOff:
                    if (_timer.GetElapseTime() >= setTime)
                    {
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Name}", $"{Name} Motor servo off timeout");
                        }
                        else
                        {
                            if (_diServoOn != null && !_diServoOn.Value)
                            {
                                _state = State.Idle;
                            }
                        }
                    }
                    break;

                case State.ServoOn:
                    if (_timer.GetElapseTime() >= setTime)
                    {
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Module}", $"{Name} Motor on timeout");
                        }
                        else
                        {
                            if (_diServoOn != null && _diServoOn.Value)
                            {
                                _state = State.Idle;
                            }
                        }
                    }
                    break;

                case State.Stopping:
                    if (_timer.GetElapseTime() >= setTime)
                    {
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _doStop.SetValue(false, out _);
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Module}", $"{Name} Motor stop timeout");
                        }
                        else
                        {
                            //if (_diMotorRun != null && !_diMotorRun.Value)
                            {
                                _doStop.SetValue(false, out _);
                                _state = State.Idle;
                            }
                        }
                    }
                    if (_timer.GetElapseTime() >= resetTime)
                    {
                        _doStop.SetValue(false, out _);
                    }
                    break;

                case State.ServoReset:
                    if (_timer.GetElapseTime() >= setTime)
                    {
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _doReset.SetValue(false, out _);
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Module}", $"{Name} Servo reset timeout");
                        }
                        if (_timer.GetElapseTime() >= resetTime)
                        {
                            _doReset.SetValue(false, out _);
                            _state = State.Idle;
                        }
                    }
                    break;

                case State.MoveToPosition:
                    var boatModule = Singleton<EquipmentManager>.Instance.Modules[ModuleName.Boat] as BoatModule;
                    if (!boatModule.CheckPrepareMove(out string reason))
                    {
                        boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
                        _doMove?.SetValue(false, out _);

                        _state = State.Idle;
                        break;
                    }

                    if (_timer.GetElapseTime() >= setTime && TargetPositionFb == (int)(ServoMovePositionSet + 0.00001))
                    {
                        if (!_doMove.Value && _timer.GetElapseTime() < resetTime)
                            _doMove.SetValue(true, out _);
                        if (_timer.GetElapseTime() >= moveTimeout)
                        {
                            _doMove.SetValue(false, out _);
                            _state = State.Idle;
                            EV.PostAlarmLog($"{Module}", $"{Name} Motor move to position timeout");
                        }
                        else if (CheckServoAtPosition())
                        {
                            _doMove.SetValue(false, out _);
                            _state = State.Idle;
                            EV.PostInfoLog($"{Module}", $"{Name} Motor move to position finish");
                        }
                    }
                    if (_timer.GetElapseTime() >= resetTime)
                    {
                        _doMove.SetValue(false, out _);
                    }
                    break;

                case State.Rotating:
                    //if (_timer.GetElapseTime() >= 100)
                    //{
                    //    var currSpeed = GetRAxisSpeed();
                    //    if (!_doMove.Value && _setSpeed == (float)(currSpeed))
                    //    {
                    //        _doCCW.SetValue(!_isCW, out _);
                    //        _doCW.SetValue(_isCW, out _);
                    //        _doMove.SetValue(true, out _);
                    //        SC.SetItemValue(_scServoMoveSpeed.PathName, _setSpeed);
                    //    }
                    //    else
                    //    {
                    //        if (_timer.GetElapseTime() >= resetTime && !_doMove.Value && _setSpeed != (float)(currSpeed))
                    //        {
                    //            EV.PostAlarmLog($"{Module}", $"{Name} Motor Rotating timeout");
                    //            _doMove.SetValue(false, out _);
                    //            _state = State.Idle;
                    //        }
                    //    }
                    //}

                    break;

                default:
                    break;
            }
            return true;
        }
        public bool SetServoHome()
        {
            ResetDO();
            if (_doHome != null)
            {
                _doHome?.SetValue(true, out _);
                _state = State.Homing;
                IsHomingState = true;
            }
            else
            {
                ServoMoveSpeedSet = (float)_scServoMoveSpeed.DoubleValue;
                ServoMovePositionSet = 1;
                //_doMove.SetValue(true, out string reason);
                _timer.Start(0);
                _state = State.MoveToPosition;
            }

            _timer.Start(0);
            LOG.Write($"{Name} set motor home");

            return true;
        }
        private void Continue()
        {
            _state = State.Idle;
            SetServoMoveTo(_lastTarget, out string reason);
        }
        public bool SetServoMoveTo(string position, out string reason, float speed = 0)
        {
            reason = string.Empty;
            if (_state != State.Idle && _state != State.Rotating)
            {
                reason = "busy";
                return false;
            }
            ResetDO();

            ServoOnOffSet = true;
            if (_scServoAcc != null)
                ServoAccSet = (float)_scServoAcc.DoubleValue;

            if (_scServoDec != null)
                ServoDecSet = (float)_scServoDec.DoubleValue;

            var target = position;
            if (IoMappingDic != null && IoMappingDic.ContainsKey(position))
            {
                target = IoMappingDic[position];
            }

            _lastTarget = target;


            switch (target)
            {
                case "Position1":
                    speed = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    ServoMoveSpeedSet = speed;
                    ServoMovePositionSet = 1;
                    //_doMove.SetValue(true, out reason);
                    _timer.Start(0);
                    _state = State.MoveToPosition;
                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                    break;
                case "CapPosition":
                case "Position2":
                    speed = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    ServoMoveSpeedSet = speed;
                    ServoMovePositionSet = 2;
                    //_doMove.SetValue(true, out reason);
                    _timer.Start(0);
                    _state = State.MoveToPosition;
                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                    break;
                case "HomePosition":
                case "Position3":
                    speed = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    ServoMoveSpeedSet = speed;
                    ServoMovePositionSet = 3;
                    //_doMove.SetValue(true, out reason);
                    _timer.Start(0);
                    _state = State.MoveToPosition;
                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                    break;
                case "CW":
                    _state = State.Rotating;
                    SetRotateSpeed(speed);
                    _doCCW.SetValue(false, out _);
                    _doCW.SetValue(true, out _);
                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                    _doMove.SetPulseValue(true, resetTime);
                    break;
                case "CCW":
                    _state = State.Rotating;
                    SetRotateSpeed(speed);
                    _doCCW.SetValue(true, out _);
                    _doCW.SetValue(false, out _);
                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                    _doMove.SetPulseValue(true, resetTime);
                    break;

            }

            return true;
        }

        private void SetRotateSpeed(float speed)
        {
            var value = Math.Round(speed, 3, MidpointRounding.AwayFromZero);
            decimal standardSpeed = 0.1M;
            bool enterSplit = false;
            for (int i = 8; i <= 47; i++)
            {
                decimal compareSpeed = Convert.ToDecimal(value);
                if (Decimal.Compare(compareSpeed, standardSpeed) > 0)
                {
                    standardSpeed = standardSpeed + 0.1M;
                    continue;
                }
                enterSplit = true;
                var binaryString = Convert.ToString(i, 2);
                int chunkSize = 1; // 每个分片的大小
                List<bool> chunks = new List<bool>();
                for (int j = 0; j < binaryString.Length - chunkSize + 1; j += chunkSize)
                {
                    string chunk = binaryString.Substring(j, chunkSize);
                    chunks.Add(chunk.Equals("1"));
                }
                chunks.Reverse();
                _doM0.SetValue(chunks[0], out _);
                _doM1.SetValue(chunks[1], out _);
                _doM2.SetValue(chunks[2], out _);
                _doM3.SetValue(chunks[3], out _);
                switch (chunks.Count)
                {
                    case 5:
                        _doM4.SetValue(chunks[4], out _);
                        _doM5.SetValue(false, out _);
                        break;
                    case 6:
                        _doM4.SetValue(chunks[4], out _);
                        _doM5.SetValue(chunks[5], out _);
                        break;
                    default:
                        _doM4.SetValue(false, out _);
                        _doM5.SetValue(false, out _);
                        break;
                }
                return;
            }
            if (!enterSplit)
            {
                _doM0.SetValue(true, out _);
                _doM1.SetValue(true, out _);
                _doM2.SetValue(false, out _);
                _doM3.SetValue(false, out _);
                _doM4.SetValue(true, out _);
                _doM5.SetValue(false, out _);
            }
        }
        public bool CheckServoAtPosition()
        {
            int target = (int)(_aoTargetPosition.FloatValue + 0.0001);
            switch (target)
            {
                case 1:
                    return _diPosition1.Value;
                case 2:
                    return _diPosition2.Value;
                case 3:
                    return _diPosition3.Value;
            }

            return false;
        }
        public bool CheckServoAtPosition(string position)
        {
            switch (position)
            {
                case "Position1":
                    return _diPosition1.Value;
                case "Position2":
                case "CapPosition":
                    return _diPosition2.Value;
                case "HomePosition":
                case "Position3":
                    return _diPosition3.Value;
            }

            return false;
        }
        public bool CheckServoHomeDone()
        {
            if (IsHomeDone && IsReady)
            {
                LOG.Write($"{Name} Check Servo HomeDone");
                return true;
            }
            return false;
        }
        public bool ServoReset(out string reason)
        {
            reason = string.Empty;
            _doReset.SetPulseValue(true, resetTime);

            return true;
        }
        public void SetServoOnOff(bool isOn)
        {
            ServoOnOffSet = isOn;
        }
        public void ServoStop()
        {
            ResetDO();
            _state = State.Idle;
            _doStop.SetPulseValue(true, resetTime);
        }

        public void SetSpeed(float value)
        {
            SetServoPara("MoveSpeed", value);
        }

        public void SetPauseResume(bool isPause)
        {
            IsPause = IsPause;
        }

        private void SetServoPara(string module, float value)
        {
            var target = module;
            if (IoMappingDic != null && IoMappingDic.ContainsKey(module))
            {
                target = IoMappingDic[module];
            }

            switch (target)
            {
                case "MoveSpeed":
                    SetServoMoveSpeed(value);
                    break;
                case "Position1":
                    SC.SetItemValue(_scServoPosition1.PathName, value);
                    break;
                case "Position2":
                    SC.SetItemValue(_scServoPosition2.PathName, value);
                    break;
                case "Position3":
                    SC.SetItemValue(_scServoPosition3.PathName, value);
                    break;
                case "Position4":
                    SC.SetItemValue(_scServoPosition4.PathName, value);
                    break;
                case "Position5":
                    SC.SetItemValue(_scServoPosition5.PathName, value);
                    break;
                case "Position6":
                    SC.SetItemValue(_scServoPosition6.PathName, value);
                    break;
                case "Position7":
                    SC.SetItemValue(_scServoPosition7.PathName, value);
                    break;
                case "Position8":
                    SC.SetItemValue(_scServoPosition8.PathName, value);
                    break;
                case "Position9":
                    SC.SetItemValue(_scServoPosition9.PathName, value);
                    break;
                case "Position10":
                    SC.SetItemValue(_scServoPosition10.PathName, value);
                    break;
            }
        }
        private void SetServoMoveSpeed(float speed)
        {
            ServoMoveSpeedSet = speed;
            SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
        }
        private void SetServoMovePosition(float position)
        {
            ServoMovePositionSet = position;
        }
        protected void ResetDO()
        {
            if (_doHome != null && _doHome.Value)
                _doHome.SetValue(false, out _);
            if (_doInit != null && _doInit.Value)
                _doInit.SetValue(false, out _);
            if (_doMove != null && _doMove.Value)
                _doMove.SetValue(false, out _);
            if (_doStop != null && _doStop.Value)
                _doStop.SetValue(false, out _);
            if (_doCW != null && _doCW.Value)
                _doCW.SetValue(false, out _);
            if (_doCCW != null && _doCCW.Value)
                _doCCW.SetValue(false, out _);
            if (_doReset != null && _doReset.Value)
                _doReset.SetValue(false, out _);
        }
    }
}