feat: Введен альтернативный алгоритм для смыкания, в случае, когда верхний электрод проезжает расстояние дольше

src/OptAlgorithm/OptAlgorithm.py

@@ -37,18 +37,40 @@ class OptAlgorithm(AutoConfigClass):
         return max(self.Ts["topen_2_mark"], self.Ts["topen_1_mark"]) - self.time_command
     
     def V1Close(self, t: float):
-        if t < self.Ts["tclose_1_acc"]:
+        if "v1close" in self.__dict__.keys():
-            return self.a_max_1 * t
+            return self.v1close(t)
-        else:
+        self.v1close = PhaseCalc(cummulative=False)
-            return self.a_max_1 * self.Ts["tclose_1_acc"]
+        xwait = lambda t: 0
+        xacc = lambda t: self.a_max_1 * t
+        v1 = (self.Ts["tclose_1_acc"]) * self.a_max_1
+        xspeed = lambda t: v1
+        xdec = lambda t: v1 - self.a_max_1 * t
+        xend = lambda t: 0
+        self.v1close.add_phase(self.Ts["tclose_1_wait"], xwait)
+        self.v1close.add_phase(self.Ts["tclose_1_acc"], xacc)
+        self.v1close.add_phase(self.Ts["tclose_1_speed"], xspeed)
+        self.v1close.add_phase(self.Ts["tclose_1_dec"], xdec)
+        self.v1close.add_phase(self.INF, xend)
+        return self.v1close(t)
 
-    def X1Close(self, t: float):
+    def X1Close(self, T: float):
-        t1 = min(t, self.Ts["tclose_1_acc"])
+        if "x1close" in self.__dict__.keys():
-        x0 =  self.a_max_1 * t1 * t1 / 2
+            return self.x1close(T)
-        
+        self.x1close = PhaseCalc(cummulative=True)
-        t2 = max(t - self.Ts["tclose_1_acc"], 0)
+        xwait = lambda t: 0
-        x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2
+        xacc = lambda t: self.a_max_1 * t * t /2
-        return x0 + x1 + self.x1_start
+        v1 = (self.Ts["tclose_1_acc"]) * self.a_max_1
+        xspeed = lambda t: v1 * t
+        xdec = lambda t: v1 * t - self.a_max_1 * t * t /2
+        xstart = lambda t: self.x1_start
+        xend = lambda t: 0
+        self.x1close.add_phase(0, xstart)
+        self.x1close.add_phase(self.Ts["tclose_1_wait"], xwait)
+        self.x1close.add_phase(self.Ts["tclose_1_acc"], xacc)
+        self.x1close.add_phase(self.Ts["tclose_1_speed"], xspeed)
+        self.x1close.add_phase(self.Ts["tclose_1_dec"], xdec)
+        self.x1close.add_phase(self.INF, xend)
+        return self.x1close(T)
         
     def V2Close(self, t: float):
         if t < self.Ts["tclose_2_acc"]:

src/OptAlgorithm/OptTimeCalculator.py

@@ -31,11 +31,13 @@ class OptTimeCalculator(AutoConfigClass):
 
         Tclose = max(T1, T2)
 
-        tclose_1_acc, tclose_1_speed = self.calcFirstClose(Tclose, h1)
+        tclose_1_wait, tclose_1_acc, tclose_1_speed, tclose_1_dec = self.calcFirstClose(Tclose, h1)
         tclose_2_acc, tclose_2_speed = self.calcSecondClose(Tclose, h2)
 
         self.allTimes["tclose_1_acc"] = tclose_1_acc
         self.allTimes["tclose_1_speed"] = tclose_1_speed
+        self.allTimes["tclose_1_wait"] = tclose_1_wait
+        self.allTimes["tclose_1_dec"] = tclose_1_dec
 
         self.allTimes["tclose_2_acc"] = tclose_2_acc
         self.allTimes["tclose_2_speed"] = tclose_2_speed
@@ -206,14 +208,23 @@ class OptTimeCalculator(AutoConfigClass):
         T = Tfull
         self.allTimes["tmovement"] = T
 
-    def calcFirstClose(self, T: float, s: float) -> tuple[float, float]:
+    def calcFirstClose(self, T: float, s: float) -> tuple[float, float, float, float]:
         v0q = min(sqrt(2 * self.a_max_1 * s), self.v_max_1)
         v0 = min(v0q, sqrt(1 / (self.k_hardness_1 * self.mass_1)) * self.Ftogrow)
-        t1 = T - sqrt(max(0, T ** 2 - 2 * s / self.a_max_1))
+        t2 = T - sqrt(max(0, T ** 2 - 2 * s / self.a_max_1))
-        if t1 > v0/ self.a_max_1 + self.check_eps:
+        if t2 > v0/ self.a_max_1 + self.check_eps:
             raise Exception("""Мы вышли за границы разгона - смыкание FE, вообще не знаю как так получилось""")
-        t2 = max(0, (s - self.a_max_1 * t1 ** 2 / 2) / (self.a_max_1 * t1))
+        if t2 * self.a_max_1 < v0:
-        return t1, t2
+            #we should wait to end with max speed
+            t2 = v0 / self.a_max_1
+            t3 = max(0, (s - self.a_max_1 * t2 ** 2 / 2) / (self.a_max_1 * t2))
+            t1 = T - t2 - t3
+            t4 = 0
+        else:
+            t1 = 0
+            t3 = max(0, (s - self.a_max_1 * t1 ** 2 / 2) / (self.a_max_1 * t1))
+            t4 = 0 #here it should be as trap
+        return t1, t2, t3, t4
 
     def calcFirstOpen(self, T: float, s: float) -> tuple[float, float]:
         t1 = T / 2 - sqrt(max(0, T ** 2 - 4 * s / self.a_max_1)) / 2