1.pm home时增加lift pin is busy检查

VirgoRT/Devices/IODevices/IoTriStateLift4.cs

@@ -34,6 +34,7 @@ namespace VirgoRT.Devices
         private readonly DIAccessor _diServo3BrakeON;
         private readonly DIAccessor _diServo4BrakeON;
         private readonly DIAccessor _diServoFSTP;
+        private readonly DIAccessor _diServoBusy;
 
         private readonly DOAccessor _doReset;
 
@@ -172,6 +173,14 @@ namespace VirgoRT.Devices
             }
         }
 
+        public bool IsBusy
+        {
+            get
+            {
+                return _diServoBusy != null ? _diServoBusy.Value : false;
+            }
+        }
+
         public IoTriStateLift4(string module, XmlElement node, string ioModule = "")
         {
             base.Module = module;
@@ -194,6 +203,7 @@ namespace VirgoRT.Devices
             _diServo3BrakeON = ParseDiNode("diServo3BrakeON", node, ioModule);
             _diServo4BrakeON = ParseDiNode("diServo4BrakeON", node, ioModule);
             _diServoFSTP = ParseDiNode("diServoFSTP", node, ioModule);
+            _diServoBusy = ParseDiNode("diServoBusy", node, ioModule);
             //_diServoAlarm = ParseDiNode("diServoAlarm", node, ioModule);
             //_diCCWLimitSensorAlarm = ParseDiNode("diCCWLimitAlarm", node, ioModule);
 
@@ -302,31 +312,31 @@ namespace VirgoRT.Devices
         public bool GoPosition(Position position)
         {
             Reset();
-            if (!_diServo1BrakeON.Value)
-            {
-                NoDuplicatedAlarm($"Lift Pin DI-{_diServo1BrakeON.Index} alarm");
-                return false;
-            }
-            if (!_diServo2BrakeON.Value)
-            {
-                NoDuplicatedAlarm($"Lift Pin DI-{_diServo2BrakeON.Index} alarm");
-                return false;
-            }
-            if (!_diServo3BrakeON.Value)
-            {
-                NoDuplicatedAlarm($"Lift Pin DI-{_diServo3BrakeON.Index} alarm");
-                return false;
-            }
-            if (!_diServo4BrakeON.Value)
-            {
-                NoDuplicatedAlarm($"Lift Pin DI-{_diServo4BrakeON.Index} alarm");
-                return false;
-            }
-            if (!_diServoFSTP.Value)
-            {
-                NoDuplicatedAlarm($"Lift Pin DI-{_diServoFSTP.Index} alarm");
-                return false;
-            }
+            //if (!_diServo1BrakeON.Value)
+            //{
+            //    NoDuplicatedAlarm($"Lift Pin DI-{_diServo1BrakeON.Index} alarm");
+            //    return false;
+            //}
+            //if (!_diServo2BrakeON.Value)
+            //{
+            //    NoDuplicatedAlarm($"Lift Pin DI-{_diServo2BrakeON.Index} alarm");
+            //    return false;
+            //}
+            //if (!_diServo3BrakeON.Value)
+            //{
+            //    NoDuplicatedAlarm($"Lift Pin DI-{_diServo3BrakeON.Index} alarm");
+            //    return false;
+            //}
+            //if (!_diServo4BrakeON.Value)
+            //{
+            //    NoDuplicatedAlarm($"Lift Pin DI-{_diServo4BrakeON.Index} alarm");
+            //    return false;
+            //}
+            //if (!_diServoFSTP.Value)
+            //{
+            //    NoDuplicatedAlarm($"Lift Pin DI-{_diServoFSTP.Index} alarm");
+            //    return false;
+            //}
 
             if (_diABSAlarm.Value)
             {

VirgoRT/Devices/JetPM.cs

@@ -155,6 +155,15 @@ namespace VirgoRT.Devices
             }
         }
 
+        public bool CheckLiftPinBusy()
+        {
+            if(_LiftPinMode == 4 && _TriStateLiftPin4 != null)
+            {
+                return _TriStateLiftPin4.IsBusy;
+            }
+            return false;
+        }
+
         public MovementPosition SmallPosition
         {
             get

VirgoRT/Modules/PMs/PMHomeRoutine.cs

@@ -2,6 +2,7 @@
 using Aitex.Core.RT.Routine;
 using VirgoRT.Devices;
 using VirgoCommon;
+using Aitex.Core.RT.Event;
 
 namespace VirgoRT.Modules.PMs
 {
@@ -28,6 +29,11 @@ namespace VirgoRT.Modules.PMs
         public Result Start(params object[] objs)
         {
             Reset();
+            if(_chamber.CheckLiftPinBusy())
+            {
+                EV.PostAlarmLog(Module, $"[{Name}] routine failed, the servo lift pin is busy now");
+                return Result.FAIL;
+            }
             Notify("Start");
             return Result.RUN;
         }

VirgoSimulator/Instances/SimulatorSystem_R.cs

@@ -100,6 +100,8 @@ namespace VirgoSimulator.Instances
             IO.DI[$"{mod}.DI_RF_Cover_Closed"].Value = true;
             IO.DI[$"{mod}.DI_CHBW_OT_SW"].Value = true;
 
+            IO.DI[$"{mod}.DI_Lift_Servo_Busy"].Value = false;
+
             ////Guide pin
             //IO.DI[$"{mod}.DI_Guide_Pin_Small_Lifter_Up_Pos"].Value = false;
             //IO.DI[$"{mod}.DI_Guide_Pin_Small_Lifter_Down_Pos"].Value = true;