diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py index 88cd752..023046c 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/src/OptAlgorithm/OptTimeCalculator.py @@ -21,8 +21,12 @@ class OptTimeCalculator(AutoConfigClass): v0q = min(sqrt(2 * self.a_max_1 * h1), self.v_max_1) v0 = min(v0q, sqrt(1 / (self.k_hardness_1 * self.mass_1)) * self.Ftogrow) t1 = v0 / self.a_max_1 - t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 / 2)) / v0) - T1 = t1 + t2t + hleft = (h1 - (self.a_max_1 * t1 * t1 / 2)) + t3 = min((v0q - v0)/ self.a_max_1, sqrt(self.a_max_1 * hleft)) + v1 = (t1 + t3) * self.a_max_1 + hleft2 = h1 - (self.a_max_1 * (t1+t3) * (t1+t3) / 2) - v1 * t3 + t3**2 * self.a_max_1 / 2 + t2t = max(0, hleft2 / v1) + T1 = t1 + t2t + t3 t21 = sqrt(h2 / (self.a_max_2)) t21 = min(self.v_max_2 / self.a_max_2, t21) @@ -178,7 +182,7 @@ class OptTimeCalculator(AutoConfigClass): 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 = -Sfull + S1 + S2max = abs(Sfull) + S1 t5 = min(t5max, (vmax) / a, sqrt(S2max / a)) t3 = abs(v0) / a + t1 + t5 t32 = t5 @@ -191,7 +195,10 @@ class OptTimeCalculator(AutoConfigClass): t2max = (timeleft - Sleft / v3) / (1 + v1 / v3) Smovement = -v0 * t1 - a / 2 * t1 ** 2 - v1 * t31 + a / 2 * t31 ** 2 - t2 = max(0, min(t2max, (abs(maxL) - abs(Smovement)) / v1)) + 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) @@ -212,8 +219,6 @@ class OptTimeCalculator(AutoConfigClass): 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)) - if t2 > v0/ self.a_max_1 + self.check_eps: - raise Exception("""Мы вышли за границы разгона - смыкание FE, вообще не знаю как так получилось""") if t2 * self.a_max_1 < v0: #we should wait to end with max speed t2 = v0 / self.a_max_1 @@ -222,8 +227,10 @@ class OptTimeCalculator(AutoConfigClass): 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 + t2 = min(v0q / self.a_max_1, (T + v0/self.a_max_1)/2 - sqrt(max(0, (T + v0/self.a_max_1)**2 - 4 * (v0**2 / (2 * self.a_max_1**2) + s / self.a_max_1))) / 2) + v1 = t2 * self.a_max_1 + t4 = max(0, v1-v0) / self.a_max_1 + t3 = max(0, T - t2 - t4) return t1, t2, t3, t4 def calcFirstOpen(self, T: float, s: float) -> tuple[float, float]: