from OptAlgorithm.OptTimeCalculator import OptTimeCalculator from OptAlgorithm.AutoConfigClass import AutoConfigClass from OptAlgorithm.ConstantCalculator import ConstantCalculator from numpy import cos, sin, sqrt, cbrt, arcsin class OptAlgorithm(AutoConfigClass): params_list = [] def __init__(self, operator_config : dict, system_config : dict): cCalculator = ConstantCalculator(operator_config, system_config) super().__init__(OptAlgorithm.params_list, operator_config, system_config, cCalculator.calc()) calc = OptTimeCalculator(operator_config, system_config) self.Ts = calc.T(self.dist_open_start_1, self.dist_open_start_2, self.dist_open_after_1, self.dist_open_after_2, self.dist_open_end_1, self.dist_open_end_2) self.x1Contact = self.dist_open_start_1 + self.position_start_1 self.x2Contact = self.dist_open_start_2 + self.position_start_2 def V1Close(self, t: float): if t < self.Ts["tclose_1_acc"]: return self.a_max_1 * t else: return self.a_max_1 * self.Ts["tclose_1_acc"] def X1Close(self, t: float): t1 = min(t, self.Ts["tclose_1_acc"]) x0 = self.a_max_1 * t1 * t1 / 2 t2 = max(t - self.Ts["tclose_1_acc"], 0) x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2 return x0 + x1 + self.position_start_1 def V2Close(self, t: float): if t < self.Ts["tclose_2_acc"]: return self.a_max_2 * t t -= self.Ts["tclose_2_acc"] if t < self.Ts["tclose_2_speed"]: return self.a_max_2 * self.Ts["tclose_2_acc"] t -= self.Ts["tclose_2_speed"] return self.a_max_2 * self.Ts["tclose_2_acc"] - self.a_max_2 * t def X2Close(self, t: float): t1 = min(t, self.Ts["tclose_2_acc"]) x0 = self.a_max_2 * t1 * t1 / 2 t2 = max(min(t - self.Ts["tclose_2_acc"], self.Ts["tclose_2_speed"]), 0) x1 = self.a_max_2 * self.Ts["tclose_2_acc"] * t2 t3 = max(min(t - self.Ts["tclose_2_speed"]- self.Ts["tclose_2_acc"], self.Ts["tclose_2_acc"]), 0) x2 = self.a_max_2 * self.Ts["tclose_2_acc"] * t3 - self.a_max_2 * t3 * t3 / 2 return x0 + x1 + x2 + self.position_start_2 def FClose(self, t: float): return 0 def V1Grow(self, t: float): F = self.FGrow(t) dF0 = self.a_max_1 * self.Ts["tclose_1_acc"] * self.k_hardness_1 dFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow) if t < self.Ts["tspeed"]: dF = sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control * sin(sqrt(self.k_hardness_1 / self.mass_1) * t) + dF0 * cos(sqrt(self.k_hardness_1 / self.mass_1) * t) return dF / self.k_hardness_1 t -= self.Ts["tspeed"] if t < self.Ts["tmeet"]: return dFmax / self.k_hardness_1 t -= self.Ts["tmeet"] if t < self.Ts["tend"]: return sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - F**2) / self.k_hardness_1 v0 = sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2) b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3 dF = 3.0*b*cbrt((F -self.force_target)/b)**2 return dF / self.k_hardness_1 def X1Grow(self, t: float): F = self.FGrow(t) x = F / self.k_hardness_1 return x + self.x1Contact def V2Grow(self, t: float): """ Считается, что верхний электрод не влияет на набор усилия, функция не реализована!, возвращает 0. Устанавливайте kturn = 0 """ return 0 def X2Grow(self, t: float): """ Считается, что верхний электрод не влияет на набор усилия, функция не реализована!, возвращает 0. Устанавливайте kturn = 0 """ return self.x2Contact def FGrow(self, t: float): v0 = self.a_max_1 * self.Ts["tclose_1_acc"] dF0 = self.a_max_1 * self.Ts["tclose_1_acc"] * self.k_hardness_1 dFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow) self.Fmeet = 1/ self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - dFmax**2) tspeed = self.Ts["tspeed"] Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * tspeed) if t < self.Ts["tspeed"]: return - self.eff_control * cos(self.freq * t) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * t) t -= self.Ts["tspeed"] if t < self.Ts["tmeet"]: return Fspeed + t * dFmax t -= self.Ts["tmeet"] if t < self.Ts["tend"]: tm = arcsin(self.Fmeet/(self.Ftogrow)) * 1/self.freq return self.Ftogrow * sin(self.freq*(t + tm)) t -= self.Ts["tend"] #tprop a_max_1 = self.force_target v0 = self.freq*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2) b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3 q = self.Fstart_prop return 3 * t**2 * cbrt(b*b*(q-a_max_1)) + 3 * t * cbrt(q-a_max_1)**2 * cbrt(b) + b * t**3 + q def V1Open(self, t: float): if t < self.Ts["topen_1_acc"]: return -self.a_max_1 * t t -= self.Ts["topen_1_acc"] if t < self.Ts["topen_1_speed"]: return -self.a_max_1 * self.Ts["topen_1_acc"] t -= self.Ts["topen_1_speed"] if t < self.Ts["topen_1_acc"]: return -self.a_max_1 * self.Ts["topen_1_acc"] + self.a_max_1 * t return 0 def X1Open(self, t: float): xm = self.force_target / self.k_hardness_1 t1 = min(t, self.Ts["topen_1_acc"]) x0 = -self.a_max_1 * t1 * t1 / 2 t2 = max(min(t - self.Ts["topen_1_acc"], self.Ts["topen_1_speed"]), 0) x1 = -self.a_max_1 * self.Ts["topen_1_acc"] * t2 t3 = max(min(t - self.Ts["topen_1_speed"]- self.Ts["topen_1_acc"], self.Ts["topen_1_acc"]), 0) x2 = -self.a_max_1 * self.Ts["topen_1_acc"] * t3 + self.a_max_1 * t3 * t3 / 2 return xm + x0 + x1 + x2 + self.x1Contact def V2Open(self, t: float): t = max(t-self.Ts["topen_2_offset"] , 0) if t < self.Ts["topen_2_acc"]: return -self.a_max_2 * t t -= self.Ts["topen_2_acc"] if t < self.Ts["topen_2_speed"]: return -self.a_max_2 * self.Ts["topen_2_acc"] t -= self.Ts["topen_2_speed"] if t < self.Ts["topen_2_acc"]: return -self.a_max_2 * self.Ts["topen_2_acc"] + self.a_max_2 * t return 0 def X2Open(self, t: float): t = max(t-self.Ts["topen_2_offset"] , 0) t1 = min(t, self.Ts["topen_2_acc"]) x0 = -self.a_max_2 * t1 * t1 / 2 t2 = max(min(t - self.Ts["topen_2_acc"], self.Ts["topen_2_speed"]), 0) x1 = -self.a_max_2 * self.Ts["topen_2_acc"] * t2 t3 = max(min(t - self.Ts["topen_2_speed"]- self.Ts["topen_2_acc"], self.Ts["topen_2_acc"]), 0) x2 = -self.a_max_2 * self.Ts["topen_2_acc"] * t3 + self.a_max_2 * t3 * t3 / 2 return x0 + x1 + x2 + self.x2Contact def FOpen(self, t: float): x1 = self.X1Open(t) x2 = self.X2Open(t) F = self.k_hardness_1 * max(0, (x1 - self.x1Contact)) return F def calcPhaseClose(self, t: float): """ Получить значения X1, X2, V1, V2, F в момент времени t для фазы смыкания Args: t (float): Момент времени Returns: (float, float, float, float, float): X1, X2, V1, V2, F """ return self.X1Close(t), self.X2Close(t), self.V1Close(t), self.V2Close(t), self.FClose(t) def calcPhaseGrow(self, t: float): """ Получить значения X1, X2, V1, V2, F в момент времени t для фазы роста усилия Args: t (float): Момент времени Returns: (float, float, float, float, float): X1, X2, V1, V2, F """ return self.X1Grow(t), self.X2Grow(t), self.V1Grow(t), self.V2Grow(t), self.FGrow(t) def calcPhaseOpen(self, t: float): """ Получить значения X1, X2, V1, V2, F в момент времени t для фазы раксрытия Args: t (float): Момент времени Returns: (float, float, float, float, float): X1, X2, V1, V2, F """ return self.X1Open(t), self.X2Open(t), self.V1Open(t), self.V2Open(t), self.FOpen(t) def getSpecific(self, param : str, phase : str, t : float): """ Получить значение величины в определенную фазу в момент времени t (с начала фазы) Args: param (str): Значение из списка X1 | X2 | V1 | V2 | F phase (str): Значение из списка: Close | Grow | Open t (float): Время Returns: Значение величины """ funcName = param + phase try: func = getattr(self, funcName) except: print("Wrong param or phase name") return 0 return func(t) def getVar(self, param : str, t : float): """ Получить значение величины в момент времени t (БЕЗ УЧЕТА СВАРКИ!) Args: param (str): Значение из списка X1 | X2 | V1 | V2 | F t (float): Время Returns: Значение величины """ if t < self.Ts["tclose"]: return self.getSpecific(param, "Close", t) t -= self.Ts["tclose"] if t < self.Ts["tgrow"] : return self.getSpecific(param, "Grow", t) t -= self.Ts["tgrow"] return self.getSpecific(param, "Open", t)