ELK/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 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 _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 = "";
        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;
        #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 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;
            }
        }
        #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);
            _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");

            _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", () => _aiRealSpeed != null ? (_isFloatAioType ? _aiRealSpeed.FloatValue : _aiRealSpeed.Value) : GetRotateSpeed());
            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;
                }
                SetServoMoveTo(param[0].ToString(), out _);
                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 || (_diHoming != null && _diHoming.Value))
                {
                    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 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
                        {
                            if (_diHomeDone != null && _diHomeDone.Value && !_doHome.Value) 
                            {
                                _doHome.SetValue(false, out _);
                                _state = State.Idle;
                                EV.PostInfoLog($"{Module}", $"{Name} Motor home finish");
                            }
                        }
                    }
                    if (_timer.GetElapseTime() >= resetTime)
                    {
                        _doHome.SetValue(false, out _);
                        _state = State.Idle;
                    }
                    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:
                    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:
                    break;

                default:
                    break;
            }
            return true;
        }
        public bool SetServoHome()
        {
            ResetDO();
            if (_doHome != null)
            {
                _doHome?.SetValue(true, out _);
                _timer.Start(0);
                _state = State.Homing;
            }
            else
            {
                ServoMoveSpeedSet = (float)_scServoMoveSpeed.DoubleValue;
                ServoMovePositionSet = 1;
                //_doMove.SetValue(true, out string reason);
                _timer.Start(0);
                _state = State.MoveToPosition;
            }
            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":
                    ServoMoveSpeedSet = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    ServoMovePositionSet = 2;
                    //_doMove.SetValue(true, out reason);
                    _timer.Start(0);
                    _state = State.MoveToPosition;
                    break;
                case "CapPosition":
                case "Position2":
                    ServoMoveSpeedSet = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    ServoMovePositionSet = 3;
                    //_doMove.SetValue(true, out reason);
                    _timer.Start(0);
                    _state = State.MoveToPosition;
                    break;
                case "HomePosition":
                case "Position3":
                    ServoMoveSpeedSet = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    ServoMovePositionSet = 1;
                    //_doMove.SetValue(true, out reason);
                    _timer.Start(0);
                    _state = State.MoveToPosition;
                    break;
                case "CW":
                    _state = State.Rotating;
                    _doMove.SetPulseValue(true, resetTime);
                    var tempSpeed = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    SetRotateSpeed(tempSpeed);
                    _doCCW.SetValue(false, out _);
                    _doCW.SetValue(true, out _);
                    break;
                case "CCW":
                    _state = State.Rotating;
                    _doMove.SetPulseValue(true, resetTime);
                    var tempSpeed1 = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
                    SetRotateSpeed(tempSpeed1);
                    _doCW.SetValue(false, out _);
                    _doCCW.SetValue(true, out _);
                    break;
                case "Rotate":
                    _state = State.Rotating;
                    ServoMovePositionSet = 0;
                    _doMove.SetPulseValue(true, resetTime);
                    _timer.Start(0);
                    break;

            }

            return true;
        }

        private void SetRotateSpeed(float speed)
        {
            decimal standardSpeed = 0.1M;
            bool enterSplit = false;
            for (int i = 8; i <= 47; i++)
            {
                decimal compareSpeed = Convert.ToDecimal(speed);
                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 _);
                _doM4.SetValue(false, out _);
                _doM5.SetValue(false, out _);
                if (chunks.Count == 5)
                {
                    _doM4.SetValue(chunks[4], out _);
                    _doM5.SetValue(false, out _);
                }
                if (chunks.Count == 6)
                {
                    _doM4.SetValue(chunks[4], out _);
                    _doM5.SetValue(chunks[5], out _);
                }
                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 _);
            }
        }

        private float GetRotateSpeed()
        {
            float tempValue = 0;
            BitArray bitArray = new BitArray(6);

            bitArray[0] = _doM0.Value;
            bitArray[1] = _doM1.Value;
            bitArray[2] = _doM2.Value;
            bitArray[3] = _doM3.Value;
            bitArray[4] = _doM4.Value;
            bitArray[5] = _doM5.Value;
            int[] res = new int[1];
            bitArray.CopyTo(res, 0);
            if (res[0] > 7)
            {
                tempValue = (float)((res[0] - 7) / 10.0);
            }
            return tempValue;

        }

        public bool CheckServoAtPosition()
        {
            int target = (int)(_aoTargetPosition.FloatValue + 0.0001);
            switch (target)
            {
                case 2:
                    return _diPosition1.Value;
                case 3:
                    return _diPosition2.Value;
                case 1:
                    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 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 _);
        }
    }
}