fix: Убрал лишний Relief

src/OptAlgorithm/OptTimeCalculator.py

@@ -142,7 +142,7 @@ class OptTimeCalculator(AutoConfigClass):
         self.Topen(s1, s2, l1, l2, self.force_target / self.k_hardness_1, 0)
         return self.allTimes
 
-    def Tmovement(self, closeAlgo, tmark) -> tuple[list, list]:
+    def Tmovement(self, closeAlgo, tmark):
         contact = [self.contact_distance_1, self.contact_distance_2]
         v0s = []
         pos0s = []
@@ -176,32 +176,38 @@ class OptTimeCalculator(AutoConfigClass):
                             проверьте distance_s_{i}, distance_h_end{i}, time_command, time_robot_movement""")
         t1max = ((Tfull + 2 * t3) + v0 / a) / (2) - sqrt(sqrtval) * sqrt(2) / (4 * a ** 2)
         t1 = min(t1max, (vmax - abs(v0)) / a)
-        t1 = max(0, min(t1, -v0 / a + sqrt(v0 ** 2 / (a ** 2) + (abs(maxL) - v0 * v0 / a) / a)))
-
+        t1Llimit = -v0 / a + sqrt(v0 ** 2 / (a ** 2) + (abs(maxL) - v0 * v0 / a) / a)
+        t1 = max(0, min(t1, t1Llimit))
         t31 = v0 / a + t1        
-        t5max = (Tfull - v0 / a) / 2 - t1
         v1 = v0 + a * t1
-        S1 = v0 * t1 + a * t1 * t1 / 2 + v1 * t31 - a * t31 * t31 / 2 + v0 * t31
-        S2max = abs(Sfull) + S1
-        t5 = min(t5max, (vmax) / a, sqrt(S2max / a))
-        t3 = abs(v0) / a + t1 + t5
-        t32 = t5
         
-        v1 = abs(v0 + t1 * a)
-        v3 = abs(v0 + t1 * a - t3 * a)
-        timeleft = Tfull - t1 - t5 - t3
-        sq = -v0 * t1 - a * t1 ** 2 / 2 - v1 * t3 + a * t3 ** 2 / 2 + v3 * t5 - a * t5 ** 2 / 2
-        Sleft = Sfull - sq
+        #try to push t2 to limit S3_2-5
+        maxiter = 10
+        t2 = 0
+        for j in range(maxiter):
+            S1 = v0 * t1 + a * t1 * t1 / 2 + v1 * t31 - a * t31 * t31 / 2 + t2 * v1
+            S2max = Sfull + S1
+            t5max = sqrt(S2max / a)
+            t5 = min(t5max, (vmax) / a)
+            t32 = t5
+            t3 = t31 + t32
 
-        t2max = (timeleft - Sleft / v3) / (1 + v1 / v3)
-        Smovement = -v0 * t1 - a / 2 * t1 ** 2 - v1 * t31 + a / 2 * t31 ** 2
-        if v1 == 0:
-            t2 = 0
-        else:
-            t2 = max(0, min(t2max, (abs(maxL) - abs(Smovement)) / v1))
-        t4 = max(0, Sleft / v3 + v1 / v3 * t2)
 
-        tstay = max(0, Tfull - t1 - t2 - t3 - t4 - t5)
+            v1 = abs(v0 + t1 * a)
+            v3 = abs(v0 + t1 * a - t3 * a)
+            timeleft = Tfull - t1 - t5 - t3
+            sq = -v0 * t1 - a * t1 ** 2 / 2 - v1 * t3 + a * t3 ** 2 / 2 + v3 * t5 - a * t5 ** 2 / 2
+            Sleft = Sfull - sq
+
+            t2max = (timeleft - Sleft / v3) / (1 + v1 / v3)
+            Smovement = -v0 * t1 - a / 2 * t1 ** 2 - v1 * t31 + a / 2 * t31 ** 2
+            if v1 == 0:
+                t2 = 0
+            else:
+                t2 = max(0, min(t2max, (abs(maxL) - abs(Smovement)) / v1))
+            t4 = max(0, Sleft / v3 + v1 / v3 * t2)
+
+            tstay = max(0, Tfull - t1 - t2 - t3 - t4 - t5)
 
         self.allTimes["tmovement_" + str(i) + "_acc"] = t1
         self.allTimes["tmovement_" + str(i) + "_speed"] = t2
@@ -215,7 +221,7 @@ class OptTimeCalculator(AutoConfigClass):
         T = Tfull
         self.allTimes["tmovement"] = T
 
-    def calcFirstClose(self, T: float, s: float) -> tuple[float, float, float, float]:
+    def calcFirstClose(self, T: float, s: 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)
         t2 = T - sqrt(max(0, T ** 2 - 2 * s / self.a_max_1))
@@ -233,21 +239,21 @@ class OptTimeCalculator(AutoConfigClass):
             t3 = max(0, T - t2 - t4)
         return t1, t2, t3, t4
 
-    def calcFirstOpen(self, T: float, s: float) -> tuple[float, float]:
+    def calcFirstOpen(self, T: float, s: float):
         t1 = T / 2 - sqrt(max(0, T ** 2 - 4 * s / self.a_max_1)) / 2
         if t1 > self.v_max_1 / 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))
         return t1, t2
 
-    def calcSecondOpen(self, T: float, s: float) -> tuple[float, float]:
+    def calcSecondOpen(self, T: float, s: float):
         t1 = T / 2 - sqrt(max(0, T ** 2 - 4 * s / self.a_max_2)) / 2
         if t1 > self.v_max_2 / self.a_max_2 + self.check_eps:
             raise Exception("""Мы вышли за границы разгона - раскрытие ME, вообще не знаю как так получилось""")
         t2 = max(0, (s - self.a_max_2 * t1 ** 2) / (self.a_max_2 * t1))
         return t1, t2
 
-    def calcSecondClose(self, T: float, s: float) -> tuple[float, float]:
+    def calcSecondClose(self, T: float, s: float):
         t1 = T / 2 - sqrt(max(0, T ** 2 - 4 * s / self.a_max_2)) / 2
         if t1 > self.v_max_2 / self.a_max_2 + self.check_eps:
             raise Exception("""Мы вышли за границы разгона - смыкание ME, вообще не знаю как так получилось""")