From 1c2a3d6daa547d903a3b0940e2100643059470a0 Mon Sep 17 00:00:00 2001
From: Harald Wolff-Thobaben <haraldwolff@Haralds-MacBook-Pro-2.local>
Date: Fri, 30 Apr 2021 11:34:46 +0200
Subject: [PATCH] Add IsSafeToEnable

---
 ln.ethercat/controller/Controller.cs          | 19 ++++++++++++++++++-
 ln.ethercat/controller/ControllerRemote.cs    |  2 ++
 .../controller/remote/StupidSerialRemote.cs   |  5 +++++
 3 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/ln.ethercat/controller/Controller.cs b/ln.ethercat/controller/Controller.cs
index eeee2cc..670364a 100644
--- a/ln.ethercat/controller/Controller.cs
+++ b/ln.ethercat/controller/Controller.cs
@@ -11,6 +11,8 @@ namespace ln.ethercat.controller
     public delegate void ControllerLogicDelegate(Controller controller);
     public delegate void ControllerStateChangeDelegate(Controller controller, ControllerStates newState);
 
+    public delegate bool ControllerIsSafeToEnable(Controller controller);
+
     public enum ControllerStates {
         NONE,
         NOTREADY,
@@ -27,6 +29,8 @@ namespace ln.ethercat.controller
         public event ControllerStateChangeDelegate OnStateChanging;
         public event ControllerStateChangeDelegate OnStateChanged;
 
+        public event ControllerIsSafeToEnable OnIsSafeToEnable;
+
         public ECMaster ECMaster { get; }
 
         [ESProperty]
@@ -169,12 +173,25 @@ namespace ln.ethercat.controller
         {
             if (ControllerState == ControllerStates.READY)
             {
-                ChangeState(ControllerStates.ENABLING);
+                if (!IsSafeToEnable())
+                    ChangeState(ControllerStates.FAULT);
+                else
+                    ChangeState(ControllerStates.ENABLING);
             } else {
                 Logging.Log(LogLevel.INFO, "Controller: EnableDrives(): Current ControllerState={0}. Refusing to enable drives", ControllerState.ToString());
             }
         }
 
+        public bool IsSafeToEnable()
+        {
+            foreach (ControllerIsSafeToEnable ciste in OnIsSafeToEnable.GetInvocationList())
+            {
+                if (!ciste(this))
+                    return false;
+            }
+            return true;
+        }
+
         [ESMethod]
         public void ClearFaults()
         {
diff --git a/ln.ethercat/controller/ControllerRemote.cs b/ln.ethercat/controller/ControllerRemote.cs
index 7236748..5eaf613 100644
--- a/ln.ethercat/controller/ControllerRemote.cs
+++ b/ln.ethercat/controller/ControllerRemote.cs
@@ -70,6 +70,8 @@ namespace ln.ethercat.controller
         protected abstract void Cycle();
         protected abstract void Shutdown();
 
+        public abstract bool IsSafeToEnable();
+
         private void CycleThread()
         {
             CycleCounter = 0;
diff --git a/ln.ethercat/controller/remote/StupidSerialRemote.cs b/ln.ethercat/controller/remote/StupidSerialRemote.cs
index 917b187..9573fd7 100644
--- a/ln.ethercat/controller/remote/StupidSerialRemote.cs
+++ b/ln.ethercat/controller/remote/StupidSerialRemote.cs
@@ -241,5 +241,10 @@ namespace ln.ethercat.controller.remote
             stopReceiverThread = true;
             serialPort.Close();
         }
+
+        public override bool IsSafeToEnable()
+        {
+            return (MainTarget==0) && (FeedTarget == 0);
+        }
     }
 }
\ No newline at end of file