Welding/WeldingSpotPerformance
9
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases 2 Wiki Activity

fix: Исправлен некорректный подсчет точки смыкания

Browse Source
This commit is contained in:
ermolaev_p 2024-12-16 14:02:42 +03:00
parent 647f137d33
commit 16416d6011
5 changed files with 41 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
params/operator_params.json
View File

@ -1,5 +1,5 @@
{ {
"dist_open_start_1": [ "distance_h_start_1": [
0.003, 0.003,
0.003, 0.003,
0.003, 0.003,
@ -15,7 +15,7 @@
0.003, 0.003,
0.003 0.003
], ],
"dist_open_start_2": [ "distance_h_start_2": [
0.005, 0.005,
0.005, 0.005,
0.005, 0.005,
@ -31,7 +31,7 @@
0.005, 0.005,
0.005 0.005
], ],
"dist_open_after_1": [ "distance_s_1": [
0.001, 0.001,
0.001, 0.001,
0.001, 0.001,
@ -47,7 +47,7 @@
0.001, 0.001,
0.001 0.001
], ],
"dist_open_after_2": [ "distance_s_2": [
0.001, 0.001,
0.001, 0.001,
0.001, 0.001,
@ -63,7 +63,7 @@
0.001, 0.001,
0.001 0.001
], ],
"dist_open_end_1": [ "distance_l_1": [
0.02, 0.02,
0.02, 0.02,
0.02, 0.02,
@ -79,7 +79,7 @@
0.02, 0.02,
0.02 0.02
], ],
"dist_open_end_2": [ "distance_l_2": [
0.033, 0.033,
0.033, 0.033,
0.033, 0.033,
@ -95,7 +95,7 @@
0.033, 0.033,
0.033 0.033
], ],
"dist_close_end_1": [ "distance_h_end1": [
0.003, 0.003,
0.003, 0.003,
0.003, 0.003,
@ -111,7 +111,7 @@
0.003, 0.003,
0.003 0.003
], ],
"dist_close_end_2": [ "distance_h_end2": [
0.005, 0.005,
0.005, 0.005,
0.005, 0.005,

4
params/system_params.json
View File

@ -41,10 +41,10 @@
"transmission_ratio_2": [ "transmission_ratio_2": [
1.0 1.0
], ],
"position_start_1": [ "contact_distance_1": [
0.02 0.02
], ],
"position_start_2": [ "contact_distance_2": [
0.081 0.081
], ],
"k_prop": [ "k_prop": [

2
src/OptAlgorithm/ConstantCalculator.py
View File

@ -14,8 +14,6 @@ class ConstantCalculator(AutoConfigClass):
def calc(self): def calc(self):
constants = {} constants = {}
#self.smin1t = self.smin1 - self.dblock / 2
#self.smin2t = self.smin2 - self.dblock / 2
constants["Fprop"] = self.k_prop * self.force_target constants["Fprop"] = self.k_prop * self.force_target
constants["freq"] = sqrt(self.k_hardness_1 / self.mass_1) constants["freq"] = sqrt(self.k_hardness_1 / self.mass_1)
constants["eff_control"] = self.torque_max_1 / self.transmission_ratio_1 constants["eff_control"] = self.torque_max_1 / self.transmission_ratio_1

35
src/OptAlgorithm/OptAlgorithm.py
View File

@ -16,16 +16,18 @@ class OptAlgorithm(AutoConfigClass):
calc = OptTimeCalculator(operator_config, system_config) calc = OptTimeCalculator(operator_config, system_config)
self.Ts = calc.T(self.dist_open_start_1, self.Ts = calc.T(self.distance_h_start_1,
self.dist_open_start_2, self.distance_h_start_2,
self.dist_open_after_1, self.distance_s_1,
self.dist_open_after_2, self.distance_s_2,
self.dist_open_end_1, self.distance_l_1,
self.dist_open_end_2) self.distance_l_2)
self.x1_start = self.contact_distance_1 - self.distance_h_start_1
self.x2_start = self.contact_distance_2 - self.distance_h_start_2
self.x1Contact = self.dist_open_start_1 + self.position_start_1 self.x1_contact = self.contact_distance_1
self.x2Contact = self.dist_open_start_2 + self.position_start_2 self.x2_contact = self.contact_distance_2
self.pos0s, self.movementV0s = calc.Tmovement(self.getSpecific, self.getMarkOpen()) self.pos0s, self.movementV0s = calc.Tmovement(self.getSpecific, self.getMarkOpen())
@ -46,8 +48,7 @@ class OptAlgorithm(AutoConfigClass):
t2 = max(t - self.Ts["tclose_1_acc"], 0) t2 = max(t - self.Ts["tclose_1_acc"], 0)
x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2 x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2
return x0 + x1 + self.x1_start
return x0 + x1 + self.position_start_1
def V2Close(self, t: float): def V2Close(self, t: float):
if t < self.Ts["tclose_2_acc"]: if t < self.Ts["tclose_2_acc"]:
@ -68,7 +69,7 @@ class OptAlgorithm(AutoConfigClass):
t3 = max(min(t - self.Ts["tclose_2_speed"]- self.Ts["tclose_2_acc"], self.Ts["tclose_2_acc"]), 0) 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 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 return x0 + x1 + x2 + self.x2_start
def FClose(self, t: float): def FClose(self, t: float):
return 0 return 0
@ -95,7 +96,7 @@ class OptAlgorithm(AutoConfigClass):
def X1Grow(self, t: float): def X1Grow(self, t: float):
F = self.FGrow(t) F = self.FGrow(t)
x = F / self.k_hardness_1 x = F / self.k_hardness_1
return x + self.x1Contact return x + self.x1_contact
def V2Grow(self, t: float): def V2Grow(self, t: float):
""" """
@ -109,7 +110,7 @@ class OptAlgorithm(AutoConfigClass):
Считается, что верхний электрод не влияет на набор усилия, Считается, что верхний электрод не влияет на набор усилия,
функция не реализована!, возвращает 0. Устанавливайте kturn = 0 функция не реализована!, возвращает 0. Устанавливайте kturn = 0
""" """
return self.x2Contact return self.x2_contact
def FGrow(self, t: float): def FGrow(self, t: float):
v0 = self.a_max_1 * self.Ts["tclose_1_acc"] v0 = self.a_max_1 * self.Ts["tclose_1_acc"]
@ -161,7 +162,7 @@ class OptAlgorithm(AutoConfigClass):
t3 = max(min(t - self.Ts["topen_1_speed"]- self.Ts["topen_1_acc"], self.Ts["topen_1_acc"]), 0) 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 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 return xm + x0 + x1 + x2 + self.x1_contact
def V2Open(self, t: float): def V2Open(self, t: float):
@ -187,18 +188,18 @@ class OptAlgorithm(AutoConfigClass):
t3 = max(min(t - self.Ts["topen_2_speed"]- self.Ts["topen_2_acc"], self.Ts["topen_2_acc"]), 0) 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 x2 = -self.a_max_2 * self.Ts["topen_2_acc"] * t3 + self.a_max_2 * t3 * t3 / 2
return x0 + x1 + x2 + self.x2Contact return x0 + x1 + x2 + self.x2_contact
def FOpen(self, t: float): def FOpen(self, t: float):
x1 = self.X1Open(t) x1 = self.X1Open(t)
x2 = self.X2Open(t) x2 = self.X2Open(t)
F = self.k_hardness_1 * max(0, (x1 - self.x1Contact)) F = self.k_hardness_1 * max(0, (x1 - self.x1_contact))
return F return F
def FMovement(self, t: float): def FMovement(self, t: float):
x1 = self.X1Movement(t) x1 = self.X1Movement(t)
x2 = self.X2Movement(t) x2 = self.X2Movement(t)
F = self.k_hardness_1 * max(0, (x1 - self.x1Contact)) F = self.k_hardness_1 * max(0, (x1 - self.x1_contact))
return F return F
def X1Movement(self, t: float): def X1Movement(self, t: float):

24
src/OptAlgorithm/OptTimeCalculator.py
View File

@ -19,7 +19,7 @@ class OptTimeCalculator(AutoConfigClass):
v0q = min(sqrt(2 * self.a_max_1 * h1), self.v_max_1) 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) v0 = min(v0q, sqrt(1/(self.k_hardness_1*self.mass_1))* self.Ftogrow)
t1 = v0 / self.a_max_1 t1 = v0 / self.a_max_1
t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 /2)) / v0q) t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 /2)) / v0)
T1 = t1 + t2t T1 = t1 + t2t
t21 = sqrt(h2/self.a_max_2) t21 = sqrt(h2/self.a_max_2)
@ -67,10 +67,10 @@ class OptTimeCalculator(AutoConfigClass):
if s1 >= l1: if s1 >= l1:
raise Exception("""S1 >= L1 - недопустимый сценарий, raise Exception("""S1 >= L1 - недопустимый сценарий,
проверьте dist_open_after_1, dist_close_end_1""") проверьте distance_s_1, distance_h_end1""")
if s2 >= l2: if s2 >= l2:
raise Exception("""S2 >= L2 - недопустимый сценарий, raise Exception("""S2 >= L2 - недопустимый сценарий,
проверьте dist_open_after_2, dist_close_end_2""") проверьте distance_s_2, distance_h_end2""")
s1 += Fs1 s1 += Fs1
topen_1_mark = sqrt(2 * s1 / self.a_max_1) topen_1_mark = sqrt(2 * s1 / self.a_max_1)
@ -134,23 +134,23 @@ class OptTimeCalculator(AutoConfigClass):
return self.allTimes return self.allTimes
def Tmovement(self, closeAlgo, tmark) -> None: def Tmovement(self, closeAlgo, tmark) -> None:
contact = [self.dist_open_start_1 + self.position_start_1, self.dist_open_start_2 + self.position_start_2] contact = [self.contact_distance_1, self.contact_distance_2]
v0s = [] v0s = []
pos0s = [] pos0s = []
for i in range(1,3): for i in range(1,3):
if tmark < 0: if tmark < 0:
raise Exception("""Отрицательное время этапа раскрытия, raise Exception("""Отрицательное время этапа раскрытия,
проверьте dist_open_after_{1,2}, time_command""") проверьте distance_s_{1,2}, time_command""")
v0 = closeAlgo("V"+str(i), "Open", tmark) v0 = closeAlgo("V"+str(i), "Open", tmark)
v0s.append(v0) v0s.append(v0)
x0 = closeAlgo("X"+str(i), "Open", tmark) x0 = closeAlgo("X"+str(i), "Open", tmark)
x1 = contact[i-1] - self.__dict__["dist_close_end_"+str(i)] x1 = contact[i-1] - self.__dict__["distance_h_end"+str(i)]
x = x1 - x0 x = x1 - x0
pos0s.append(closeAlgo("X"+str(i), "Open", tmark)) pos0s.append(closeAlgo("X"+str(i), "Open", tmark))
Tfull = self.time_robot_movement Tfull = self.time_robot_movement
L = self.__dict__["dist_open_end_"+str(i)] L = self.__dict__["distance_l_"+str(i)]
maxL = contact[i-1] - L - x0 maxL = contact[i-1] - L - x0
self.Tmovementi(i, x, Tfull, v0, maxL) self.Tmovementi(i, x, Tfull, v0, maxL)
return pos0s, v0s return pos0s, v0s
@ -164,7 +164,7 @@ class OptTimeCalculator(AutoConfigClass):
sqrtval = a**2 * (a**2 * (Tfull+2*t3)**2 - 8 * a * Sfull + 2 * a* v0 * (Tfull+2*t3) - 3 *v0**2) sqrtval = a**2 * (a**2 * (Tfull+2*t3)**2 - 8 * a * Sfull + 2 * a* v0 * (Tfull+2*t3) - 3 *v0**2)
if sqrtval < 0: if sqrtval < 0:
raise Exception("""Невозможно с S_{i} добраться но H*_{i} за указанное время, raise Exception("""Невозможно с S_{i} добраться но H*_{i} за указанное время,
проверьте dist_open_after_{i}, dist_close_end_{i}, time_command, time_robot_movement""") проверьте 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) t1max = ((Tfull+2*t3) + v0/a)/(2) - sqrt(sqrtval) * sqrt(2)/(4*a**2)
t1 = min(t1max, (vmax- abs(v0))/a) 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))) t1 = max(0, min(t1, -v0/a + sqrt(v0**2 / (a**2) + (abs(maxL)-v0*v0/a)/a)))
@ -208,25 +208,25 @@ class OptTimeCalculator(AutoConfigClass):
v0 = min(v0q, sqrt(1/(self.k_hardness_1*self.mass_1))* self.Ftogrow) v0 = min(v0q, sqrt(1/(self.k_hardness_1*self.mass_1))* self.Ftogrow)
t1 = T - sqrt(max(0, T**2 - 2 * s / self.a_max_1)) t1 = T - sqrt(max(0, T**2 - 2 * s / self.a_max_1))
t1 = min(t1, v0 / self.a_max_1) t1 = min(t1, v0 / self.a_max_1)
t2 = sqrt(max(0, T**2 - 2 * s / self.a_max_1)) t2 = max(0, (s - self.a_max_1*t1**2/2) / (self.a_max_1*t1))
return t1, t2 return t1, t2
def calcFirstOpen(self, T : float, s : float) -> tuple[float, float]: def calcFirstOpen(self, T : float, s : float) -> tuple[float, float]:
t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_1)) / 2 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) t1 = min(t1, self.v_max_1 / self.a_max_1)
t2 = sqrt(max(0, T * T - 4 * s / self.a_max_1)) t2 = max(0, (s - self.a_max_1*t1**2/2) / (self.a_max_1*t1))
return t1, t2 return t1, t2
def calcSecondOpen(self, T : float, s : float) -> tuple[float, float]: def calcSecondOpen(self, T : float, s : float) -> tuple[float, float]:
t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2 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) t1 = min(t1, self.v_max_2 / self.a_max_2)
t2 = sqrt(max(0, T * T - 4 * s / self.a_max_2)) t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1))
return t1, t2 return t1, t2
def calcSecondClose(self, T : float, s : float) -> tuple[float, float]: def calcSecondClose(self, T : float, s : float) -> tuple[float, float]:
t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2 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) t1 = min(t1, self.v_max_2 / self.a_max_2)
t2 = sqrt(max(0, T * T - 4 * s / self.a_max_2)) t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1))
return t1, t2 return t1, t2
def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float) -> float: def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float) -> float: