From 1d95eef744525db183003f8f1772f0722f0ab75d Mon Sep 17 00:00:00 2001
From: Sven Augustin <sven.augustin@psi.ch>
Date: Thu, 16 Feb 2023 17:31:48 +0100
Subject: [PATCH] added unstuck logic

---
 devices/undulator.py | 51 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 51 insertions(+)

diff --git a/devices/undulator.py b/devices/undulator.py
index 8f50d48..6a6d378 100644
--- a/devices/undulator.py
+++ b/devices/undulator.py
@@ -1,4 +1,5 @@
 from time import sleep
+from datetime import datetime
 import numpy as np
 from epics import PV
 
@@ -159,6 +160,11 @@ class Undulators(Adjustable):
         return any(a.is_moving() for a in self.adjs)
 
 
+    def unstuck(self):
+        for a in self.adjs.values():
+            a.unstuck()
+
+
 
 class Undulator(PVAdjustable):
 
@@ -172,6 +178,51 @@ class Undulator(PVAdjustable):
 #        self.adj_radial_on_proc = PVAdjustable(name + ":RADIAL-ON.PROC", internal=True)
 #        self.adj_radial_go_proc = PVAdjustable(name + ":RADIAL-GO.PROC", internal=True)
 
+
+    def set_target_value(self, value):
+        self.wait_for_no_alarm()
+        if self.pvs.readback.status != 0:
+            self.unstuck()
+        return super().set_target_value(value)
+
+
+    def wait_for_no_alarm(self):
+        for i in range(10):
+            if self.pvs.readback.status == 0:
+                break
+            if i == 0:
+                print(self.name, datetime.now())
+            print(self.name, f"seems stuck ({i}) 💩")
+            sleep(0.5)
+
+
+    def unstuck(self, delta=0.0001):
+        if self.pvs.readback.status == 0:
+            print(self.name, "is not stuck")
+            return
+
+        current = self.pvs.setvalue.get()
+        temp = current + delta
+        self._set_set_value(temp)
+        self._set_set_value(current)
+
+        if self.pvs.readback.status == 0:
+            print(self.name, "is not stuck anymore 😍")
+        else:
+            print(self.name, "is still stuck 😭")
+
+
+    def _set_set_value(self, value):
+        self.pvs.setvalue.put(value)
+        sleep(0.1)
+        self.adj_radial_on.set_target_value(0)
+        sleep(0.5)
+        self.adj_radial_on.set_target_value(1)
+        sleep(0.5)
+        self.adj_radial_go.set_target_value(1)
+        sleep(3)
+
+
     @property
     def energy(self):
         return self.adj_energy.get_current_value() * 1000