R Axis根据DIM0/1/2//3/4/5 倒推实际速度

Furnace/FurnaceRT/Devices/IoFurnaceMotor.cs

@@ -10,6 +10,7 @@ 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;
@@ -121,6 +122,7 @@ namespace FurnaceRT.Devices
         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
@@ -327,7 +329,7 @@ namespace FurnaceRT.Devices
             _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();
         }
@@ -335,7 +337,7 @@ namespace FurnaceRT.Devices
         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}.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);
@@ -373,7 +375,12 @@ namespace FurnaceRT.Devices
                     EV.PostWarningLog($"{Module}", $"{Name} busy, wait");
                     return false;
                 }
-                SetServoMoveTo(param[0].ToString(), out _);
+                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) =>
@@ -425,7 +432,48 @@ namespace FurnaceRT.Devices
                 _doServoOn.SetValue(true, out _);
             return true;
         }
+        public float GetSpeedFromOutputs()
+        {
+            if (_diMoving.Value || _diHoming.Value)
+            {
+                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;
+                }
+            }
+            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)
@@ -454,6 +502,7 @@ namespace FurnaceRT.Devices
             }
         }
 
+
         public virtual void Reset()
         {
             ServoReset(out _);
@@ -683,6 +732,7 @@ namespace FurnaceRT.Devices
                     //_doMove.SetValue(true, out reason);
                     _timer.Start(0);
                     _state = State.MoveToPosition;
+                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                     break;
                 case "CapPosition":
                 case "Position2":
@@ -691,6 +741,7 @@ namespace FurnaceRT.Devices
                     //_doMove.SetValue(true, out reason);
                     _timer.Start(0);
                     _state = State.MoveToPosition;
+                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                     break;
                 case "HomePosition":
                 case "Position3":
@@ -699,6 +750,7 @@ namespace FurnaceRT.Devices
                     //_doMove.SetValue(true, out reason);
                     _timer.Start(0);
                     _state = State.MoveToPosition;
+                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                     break;
                 case "CW":
                     _state = State.Rotating;
@@ -707,6 +759,7 @@ namespace FurnaceRT.Devices
                     SetRotateSpeed(speed);
                     _doCCW.SetValue(false, out _);
                     _doCW.SetValue(true, out _);
+                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                     break;
                 case "CCW":
                     _state = State.Rotating;
@@ -715,6 +768,7 @@ namespace FurnaceRT.Devices
                     SetRotateSpeed(speed);
                     _doCW.SetValue(false, out _);
                     _doCCW.SetValue(true, out _);
+                    SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
                     break;
                 case "Rotate":
                     _state = State.Rotating;