Calculation bug fix (division by zero) if d1=d2

bliss/controllers/motors/tabsup.py

@@ -35,22 +35,27 @@ class tabsup(CalcController):
         CalcController.__init__(self, *args, **kwargs)
 
         self.d1 = self.config.get("d1", float)
-        self.d2 = self.config.get("d2", float)
+        try:
+            self.d2 = self.config.get("d2", float)
+        except KeyError:
+            self.d2 = self.d1
 
     def calc_from_real(self, positions_dict):
         tyf = positions_dict["front"]
         tyb = positions_dict["back"]
-        d1 = self.d1
-        d2 = self.d2
 
-        return {"ttrans": (d1*tyb-d2*tyf)/(d1-d2),
-                "trot": math.atan((tyf-tyb)/(d2-d1)) }
+        # check if distances are identical
+        if abs(self.d1-self.d2) < 0.01:
+            dd = self.d1
+        else:
+            dd = self.d1-self.d2
+
+        return {"ttrans": (self.d1*tyb-self.d2*tyf)/(dd),
+                "trot": math.atan((tyf-tyb)/(-dd))}
 
     def calc_to_real(self, positions_dict):
         ttrans = positions_dict["ttrans"]
         trot = positions_dict["trot"]
-        d1 = self.d1
-        d2 = self.d2
 
-        return {"back": ttrans - d2*math.tan(trot),
-                "front": ttrans - d1*math.tan(trot) }
+        return {"back": ttrans - self.d2*math.tan(trot),
+                "front": ttrans - self.d1*math.tan(trot)}