from numpy import sqrt, arcsin, arccos, cos, sin from OptAlgorithm.AutoConfigClass import AutoConfigClass from OptAlgorithm.ConstantCalculator import ConstantCalculator class OptTimeCalculator(AutoConfigClass): params_list = [] def __init__(self, operator_config : dict, system_config : dict): cCalculator = ConstantCalculator(operator_config, system_config) super().__init__(OptTimeCalculator.params_list, operator_config, system_config, cCalculator.calc()) self.allTimes = {} def tGrowNominal(self, F : float): return arcsin(F/(self.Ftogrow)) * sqrt(self.mass_1/self.k_hardness_1) def Tclose(self, h1: float, h2: float): 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)) / v0q) T1 = t1 + t2t t21 = sqrt(h2/self.a_max_2) t21 = min(self.v_max_2/self.a_max_2, t21) t22 = max(0, (h2 - (self.a_max_2 * t21 * t21)) / self.v_max_2) T2 = t22 + 2 * t21 Tclose = max(T1, T2) tclose_1_acc, tclose_1_speed = 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_2_acc"] = tclose_2_acc self.allTimes["tclose_2_speed"] = tclose_2_speed self.allTimes["tclose"] = Tclose def Topen(self, s1 : float, s2 : float, l1 : float, l2 : float, Fs1 : float, Fs2 : float = 0): t11 = sqrt((l1 + Fs1)/self.a_max_1) t11 = min(self.v_max_1/self.a_max_1, t11) t12 = max(0, ((l1+Fs1) - (self.a_max_1 * t11 * t11)) / self.v_max_1) T1 = t12 + 2 * t11 offset = self.calcSecondOpenOffset(t11, t12, Fs1) t21 = sqrt(l2/self.a_max_2) t21 = min(self.v_max_2/self.a_max_2, t21) t22 = max(0, (l2 - (self.a_max_2 * t21 * t21)) / self.v_max_2) T2 = t22 + 2 * t21 + offset Topen = max(T1, T2) topen_1_acc, topen_1_speed = self.calcFirstOpen(Topen, l1+Fs1) offset = self.calcSecondOpenOffset(topen_1_acc, topen_1_speed, Fs1) topen_2_acc, topen_2_speed = self.calcSecondOpen(Topen - offset, l2) self.allTimes["topen_1_acc"] = topen_1_acc self.allTimes["topen_2_offset"] = offset self.allTimes["topen_1_acc"] = topen_1_acc self.allTimes["topen_1_speed"] = topen_1_speed self.allTimes["topen_2_acc"] = topen_2_acc self.allTimes["topen_2_speed"] = topen_2_speed if s1 > l1: raise ValueError("S1 > L1 - недопустимый сценарий") if s2 > l2: raise ValueError("S2 > L2 - недопустимый сценарий") s1 += Fs1 topen_1_mark = sqrt(2 * s1 / self.a_max_1) if topen_1_mark > topen_1_acc: s1 -= topen_1_acc ** 2 * self.a_max_1 / 2 v1 = topen_1_acc * self.a_max_1 if s1 > topen_1_speed * v1: s1 -= topen_1_speed * v1 topen_1_mark = 2*topen_1_acc + topen_1_speed - sqrt(topen_1_acc**2 - 2*s1 / self.a_max_1) else: topen_1_mark = topen_1_acc + s1 / v1 topen_2_mark = sqrt(2 * s2 / self.a_max_2) if topen_2_mark > topen_2_acc: s2 -= topen_2_acc ** 2 * self.a_max_2 / 2 v2 = topen_2_acc * self.a_max_2 if s2 > topen_2_speed * v2: s2 -= topen_2_speed * v2 topen_2_mark = 2*topen_2_acc + topen_2_speed - sqrt(topen_2_acc**2 - 2*s2 / self.a_max_2) else: topen_2_mark = topen_2_acc + s2 / v2 self.allTimes["topen_1_mark"] = topen_1_mark self.allTimes["topen_2_mark"] = topen_2_mark self.allTimes["topen"] = Topen def Tgrow(self): v0 = self.allTimes["tclose_1_acc"] * self.a_max_1 vF0 = v0 * self.k_hardness_1 vFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow) l = sqrt(self.eff_control ** 2 + self.mass_1/self.k_hardness_1 * vF0**2) L = sqrt(self.k_hardness_1 / self.mass_1 * self.eff_control ** 2 + vF0*vF0) tspeed = sqrt(self.mass_1/self.k_hardness_1) * (arcsin(vFmax / L) - arccos(sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control / L)) Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * vF0 * sin(self.freq * tspeed) Fmeet = 1/self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - vFmax**2) Fstart_prop = self.Fstart_prop tmeet = (Fmeet - Fspeed)/vFmax tend = self.tGrowNominal(Fstart_prop) - self.tGrowNominal(Fmeet) vp = 1/sqrt(self.k_hardness_1 * self.mass_1) * sqrt(self.Ftogrow**2 - self.Fstart_prop**2) ap = Fstart_prop / self.mass_1 tprop = 2*vp / ap self.allTimes["tspeed"] = tspeed self.allTimes["tmeet"] = tmeet self.allTimes["tend"] = tend self.allTimes["tprop"] = tprop self.allTimes["tgrow"] = tspeed + tmeet + tend + tprop def T(self, h1 : float, h2 : float, s1 : float, s2 : float, l1 : float, l2 : float): self.Tclose(h1, h2) self.Tgrow() self.Topen(s1, s2, l1, l2, self.force_target / self.k_hardness_1, 0) return self.allTimes def calcFirstClose(self, T : float, s : float): t1 = T - sqrt(max(0, T**2 - 2 * s / self.a_max_1)) t1 = min(t1, self.v_max_1 / self.a_max_1) t2 = sqrt(max(0, T**2 - 2 * s / self.a_max_1)) return t1, t2 def calcFirstOpen(self, T : float, s : float): t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_1)) / 2 t1 = min(t1, self.v_max_1 / self.a_max_1) t2 = sqrt(max(0, T * T - 4 * s / self.a_max_1)) return t1, t2 def calcSecondOpen(self, T : float, s : float): t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2 t1 = min(t1, self.v_max_2 / self.a_max_2) t2 = sqrt(max(0, T * T - 4 * s / self.a_max_2)) return t1, t2 def calcSecondClose(self, T : float, s : float): t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2 t1 = min(t1, self.v_max_2 / self.a_max_2) t2 = sqrt(max(0, T * T - 4 * s / self.a_max_2)) return t1, t2 def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float): s = sq * 1 offset = sqrt(2 * s / self.a_max_1) if offset > t1: s -= t1 ** 2 * self.a_max_1 / 2 v1 = t1 * self.a_max_1 if s > t2 * v1: s -= t2 * v1 print(s, t1 ** 2 * self.a_max_1/2) offset = 2*t1 + t2 - sqrt(t1**2 - 2*s / self.a_max_1) else: offset = t1 + s / v1 return offset