btl143/project_main
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: 修復hanoi2右側圖像上下翻轉時過濾條件錯誤的情況

Browse Source 
pref: 6_9較好參數
pref: 關閉hanoi2循跡模型切換
This commit is contained in:
bmy
2024-07-15 16:55:03 +08:00
parent 1edd292ac6
commit 527fa4b04b
8 changed files with 83 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
subtask.py
View File

@@ -240,6 +240,9 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
break
return True
# 对准应知道是左还是右，右侧需在过滤器中进行翻转
# flipv 为垂直翻转标志，转右侧开启
def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5):
stop_error = 0
error_record = CountRecord(10)
@@ -253,16 +256,19 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
else:
stop_error = 15
while True:
ret1, ret2, box = filter.get_two(target_label, error_label)
ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
while not ret1:
# 如果找不到目标且跳过任务队列非空 (即指令跳过)
if not global_skip_queue.empty():
_ = global_skip_queue.get()
logger.error("跳过 hanoi_calibrate")
return False
# 如果找不到目标且发现错误目标 (上次放置任务失败)
if ret2:
# 如果连续计数超过阈值，则直接返回
if error_record(ret2):
return False
ret1, ret2, box = filter.get_two(target_label, error_label)
ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret1:
@@ -916,7 +922,8 @@ class put_hanoi1():
# by_cmd.send_angle_omega(-25,430)
# by_cmd.send_angle_omega(-55,194)
# by_cmd.send_angle_omega(-45,238)
by_cmd.send_angle_omega(-45,252)
# by_cmd.send_angle_omega(-45,252)
by_cmd.send_angle_omega(-45,260)
time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
@@ -925,7 +932,8 @@ class put_hanoi1():
# by_cmd.send_angle_omega(25,430)
# by_cmd.send_angle_omega(55,194)
# by_cmd.send_angle_omega(45,238)
by_cmd.send_angle_omega(45,252)
# by_cmd.send_angle_omega(45,252)
by_cmd.send_angle_omega(45,260)
time.sleep(2)
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
@@ -934,9 +942,9 @@ class put_hanoi1():
def nexec(self):
pass
def after(self):
var.switch_lane_model = True
# var.switch_lane_model = True
if utils.direction == tlabel.RMARK:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
else:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass
@@ -1057,7 +1065,7 @@ class put_hanoi2():
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(60)
by_cmd.send_angle_claw(55)
time.sleep(2)
by_cmd.send_angle_claw(35)
time.sleep(0.5)
@@ -1069,7 +1077,7 @@ class put_hanoi2():
else:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(60)
by_cmd.send_angle_claw(55)
time.sleep(2)
by_cmd.send_angle_claw(35)
time.sleep(0.5)
@@ -1092,7 +1100,7 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
by_cmd.send_angle_claw(65)
by_cmd.send_angle_claw(55)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1)
@@ -1104,7 +1112,7 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
by_cmd.send_angle_claw(65)
by_cmd.send_angle_claw(55)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
@@ -1140,7 +1148,7 @@ class put_hanoi2():
time.sleep(2)
pass
# ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5, )
ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return
@@ -1175,16 +1183,17 @@ class put_hanoi2():
time.sleep(1.5)
# while True:
# pass
by_cmd.send_speed_x(12)
time.sleep(1.5)
def nexec(self):
pass
def after(self):
var.switch_lane_model = False
# var.switch_lane_model = False
if utils.direction is tlabel.RMARK:
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"] - 0.2, cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
else:
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
# time.sleep(2)
pass
class put_hanoi3():