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

備份 6_9 subtask 參數

Browse Source
This commit is contained in:
bmy
2024-07-15 17:04:28 +08:00
parent 527fa4b04b
commit 6205fabf34
1 changed files with 161 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

239
subtask_69.py
View File

@@ -118,7 +118,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
logger.info(f"calibrate_right_new:停车后的误差是{error}")
if abs(error) > 8:
logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
error = error * 3
error = error * 1.5
if error > 0:
by_cmd.send_distance_x(-10, int(error))
else:
@@ -241,6 +241,49 @@ 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)
if run_direc == 1:
by_cmd.send_speed_x(run_speed)
else:
by_cmd.send_speed_x(-run_speed)
if target_label == tlabel.TPLATFORM:
stop_error = 8
else:
stop_error = 15
while True:
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_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret1:
# 校准速度越大 停车的条件越宽泛 20 15
if abs(error) <= stop_error:
car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label)
while not ret:
ret, box = filter.get(target_label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
break
return True
# 任务类
class task:
def __init__(self, name, task_template, find_counts=10, enable=True):
@@ -406,7 +449,7 @@ class get_block1():
else:
var.first_block = tlabel.BBLOCK
var.second_block = tlabel.RBLOCK
calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5)
calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
logger.info("抓取块")
by_cmd.send_position_axis_z(30, 60)
@@ -421,20 +464,21 @@ class get_block1():
by_cmd.send_angle_claw_arm(175)
time.sleep(0.1)
by_cmd.send_position_axis_x(1, 120)
by_cmd.send_position_axis_x(1, 100)
time.sleep(1)
by_cmd.send_position_axis_z(30, 70)
time.sleep(0.1)
time.sleep(0.5)
by_cmd.send_angle_claw(63)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 130)
time.sleep(1)
by_cmd.send_position_axis_x(1, 140)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw_arm(225)
time.sleep(0.5)
by_cmd.send_angle_storage(55)
time.sleep(1)
# by_cmd.send_angle_storage(55)
# time.sleep(1)
by_cmd.send_position_axis_z(30, 60)
pass
def nexec(self):
@@ -463,13 +507,12 @@ class get_block2():
return False
def exec(self):
car_stop()
calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5)
calibrate_new(var.second_block, offset = 16, run = True, run_speed = 5)
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw_arm(225)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_z(30, 60)
time.sleep(1)
time.sleep(0.1)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
by_cmd.send_angle_claw(25)
@@ -521,7 +564,7 @@ class put_block():
# 放置第二個塊
by_cmd.send_angle_storage(20)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 110)
by_cmd.send_position_axis_z(30, 120)
time.sleep(1.5)
by_cmd.send_angle_claw_arm(180)
@@ -535,7 +578,7 @@ class put_block():
time.sleep(1)
by_cmd.send_position_axis_z(30, 110)
time.sleep(1)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw_arm(225)
time.sleep(1)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
@@ -554,7 +597,7 @@ class put_block():
time.sleep(1)
while by_cmd.send_position_axis_x(1, 0) == -1:
pass
while by_cmd.send_angle_claw_arm(36) == -1:
while by_cmd.send_angle_claw_arm(45) == -1:
pass
# 任务检查间隔
# time.sleep(2)
@@ -569,7 +612,7 @@ class get_bball():
# time.sleep(0.5)
# by_cmd.send_position_axis_x(1, 0)
# time.sleep(2)
# by_cmd.send_angle_claw_arm(36)
# by_cmd.send_angle_claw_arm(45)
while (by_cmd.send_angle_storage(0) == -1):
by_cmd.send_angle_storage(0)
@@ -582,13 +625,14 @@ class get_bball():
self.record = CountRecord(5)
def find(self):
# 目标检测蓝球
ret = filter.find(tlabel.BBALL)
# ret = filter.find(tlabel.BBALL)
ret = filter.find_mult([tlabel.BBALL, tlabel.YBALL])
# ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
# TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
# if ret:
# if self.record(tlabel.BBALL):
# return True
if ret:
if ret[0] or ret[1]:
return True
return False
def exec(self):
@@ -596,10 +640,10 @@ class get_bball():
car_stop()
time.sleep(0.5)
for _ in range(3):
calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5)
calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
logger.info("抓蓝色球")
time.sleep(0.5)
by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_claw_arm(45)
by_cmd.send_angle_claw(54)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1.2)
@@ -615,7 +659,7 @@ class get_bball():
time.sleep(0.5)
by_cmd.send_angle_claw(54)
time.sleep(0.5)
by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_claw_arm(45)
time.sleep(1)
by_cmd.send_position_axis_z(30, 135)
# 继续向前走
@@ -834,30 +878,40 @@ class put_hanoi1():
logger.info("应该向左转")
# # 校准 omega
# if error > 0:
# by_cmd.send_angle_omega(-20,abs(var.lane_error))
# else:
# by_cmd.send_angle_omega(20,abs(var.lane_error))
# time.sleep(0.5)
# car_stop()
# time.sleep(0.5)
# by_cmd.send_distance_x(10, 200)
# by_cmd.send_distance_x(10, 180)
by_cmd.send_distance_x(10, 250)
time.sleep(1)
# 校准 omega
for _ in range(10):
ret, box = filter.get(utils.direction)
if ret:
error = (box[0][2] + box[0][0] - 320) / 2
by_cmd.send_speed_omega(-error * 0.8)
time.sleep(0.2)
car_stop()
# 前进
# by_cmd.send_distance_x(10, 200)
# by_cmd.send_distance_x(10, 180)
# by_cmd.send_distance_x(10, 180)
# time.sleep(1.5)
# car_stop()
while True:
by_cmd.send_speed_x(8.5)
ret, box = filter.get(utils.direction)
if ret:
if abs(box[0][2] - box[0][0]) > 41:
car_stop()
break
# 根据方向初始化执行器位置
if utils.direction is tlabel.RMARK:
# FIXME 右侧的爪子会被 storage 挡住
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_claw_arm(45)
by_cmd.send_angle_storage(0)
else:
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw_arm(225)
by_cmd.send_angle_storage(55)
time.sleep(1)
@@ -866,16 +920,20 @@ class put_hanoi1():
if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK
# by_cmd.send_angle_omega(-25,430)
by_cmd.send_angle_omega(-45,238)
# 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,260)
time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
else:
utils.direction = tlabel.LMARK
# by_cmd.send_angle_omega(25,430)
by_cmd.send_angle_omega(45,238)
# 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,260)
time.sleep(2)
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
@@ -884,11 +942,17 @@ class put_hanoi1():
def nexec(self):
pass
def after(self):
# var.switch_lane_model = True
if utils.direction == tlabel.RMARK:
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
# time.sleep(1.5)
class put_hanoi2():
def __init__(self):
if cfg['put_hanoi2']['first_target'] == "lp":
self.target_label = tlabel.LPILLER
elif cfg['put_hanoi2']['first_target'] == "mp":
@@ -897,20 +961,25 @@ class put_hanoi2():
self.target_label = tlabel.SPILLER
def init(self):
logger.info("物资盘点 2 初始化")
var.task_speed = 8.5
if utils.direction == tlabel.RMARK:
# 15
self.offset = 19
self.offset = 14
# self.platform_offset = -25
self.platform_offset = -10
else:
self.offset = 10
#self.platform_offset = -30
# self.platform_offset = -19
self.platform_offset = -15
else:
self.offset = 14
# self.platform_offset = -30
# self.platform_offset = -19
self.platform_offset = -15
# 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异）
time.sleep(1.5)
def find(self):
# ret, box = filter.get(self.target_label)
ret, box = filter.get(tlabel.TPLATFORM)
if ret:
var.task_speed = 8.5
error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
if error > 0:
return True
@@ -935,12 +1004,12 @@ class put_hanoi2():
logger.info("抓大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(63)
time.sleep(2)
by_cmd.send_angle_claw(40)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, 20)
by_cmd.send_distance_axis_z(30, 30)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1)
@@ -952,7 +1021,7 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_angle_claw(40)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, 20)
by_cmd.send_distance_axis_z(30, 30)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
@@ -964,15 +1033,15 @@ class put_hanoi2():
time.sleep(0.5)
logger.info("放大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
time.sleep(1.5)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
by_cmd.send_angle_claw(81)
time.sleep(0.5)
by_cmd.send_angle_claw(63)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(0.5)
by_cmd.send_angle_claw(63)
time.sleep(1)
pass
else:
@@ -982,9 +1051,9 @@ class put_hanoi2():
time.sleep(1)
by_cmd.send_angle_claw(81)
time.sleep(0.5)
by_cmd.send_angle_claw(63)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(0.5)
by_cmd.send_angle_claw(63)
time.sleep(1)
ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
@@ -995,8 +1064,8 @@ class put_hanoi2():
logger.info("抓中平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(55)
time.sleep(2)
by_cmd.send_angle_claw(35)
time.sleep(0.5)
@@ -1008,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(63)
by_cmd.send_angle_claw(55)
time.sleep(2)
by_cmd.send_angle_claw(35)
time.sleep(0.5)
@@ -1017,32 +1086,33 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
pass
ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
# ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("跳过物资盘点 2 exec")
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("放中平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 100)
by_cmd.send_position_axis_z(30, 120)
time.sleep(2)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
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)
pass
else:
by_cmd.send_position_axis_z(30, 100)
by_cmd.send_position_axis_z(30, 120)
time.sleep(2)
by_cmd.send_position_axis_x(1, 40)
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)
@@ -1055,15 +1125,15 @@ class put_hanoi2():
logger.info("抓小平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(50)
time.sleep(2)
by_cmd.send_angle_claw(27)
time.sleep(1)
by_cmd.send_distance_axis_z(30, 10)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(2)
pass
else:
by_cmd.send_position_axis_z(30, 10)
@@ -1074,19 +1144,20 @@ class put_hanoi2():
time.sleep(1)
by_cmd.send_distance_axis_z(30, 10)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
by_cmd.send_position_axis_x(1, 170)
time.sleep(2)
pass
ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
# 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)
if not ret:
logger.error("跳过物资盘点 2 exec")
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return
time.sleep(0.5)
logger.info("放小平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 170)
by_cmd.send_position_axis_z(30, 190) # 170
time.sleep(1.5)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
@@ -1094,12 +1165,13 @@ class put_hanoi2():
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1)
by_cmd.send_speed_y(15)
time.sleep(0.1)
car_stop()
# by_cmd.send_speed_y(10)
# time.sleep(0.12)
# car_stop()
pass
else:
by_cmd.send_position_axis_z(30, 170)
by_cmd.send_position_axis_z(30, 190)
time.sleep(1.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
@@ -1111,11 +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
if utils.direction is tlabel.RMARK:
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():
@@ -1130,10 +1208,10 @@ class put_hanoi3():
while by_cmd.send_position_axis_x(1, 0) == -1:
pass
time.sleep(1)
# while by_cmd.send_angle_claw_arm(220) == -1:
# pass
# while by_cmd.send_angle_claw(90) == -1:
# while by_cmd.send_angle_claw_arm(225) == -1:
# pass
while by_cmd.send_angle_claw(85) == -1:
pass
def find(self):
time.sleep(1)
return True
@@ -1209,7 +1287,7 @@ class move_area1():
# 任务检查间隔
by_cmd.send_position_axis_x(1, 150)
# time.sleep(1)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw_arm(225)
pass
@@ -1387,11 +1465,12 @@ class kick_ass():
else:
target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
by_cmd.send_distance_x(10, target_distance)
logger.info(f"target distance {target_distance}")
time.sleep(1.5 + (self.target_person - 1) * 0.7 )
car_stop()
# by_cmd.send_angle_claw_arm(220)
# by_cmd.send_angle_claw_arm(225)
# time.sleep(0.5)
by_cmd.send_position_axis_x(1, 20)
time.sleep(3)