263 lines
10 KiB
Python
263 lines
10 KiB
Python
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)
|
||
|
||
|