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

pref: 部分动作优化

Browse Source
This commit is contained in:
bmy
2024-06-24 22:05:11 +08:00
parent 90b888eac7
commit 066ef112ad
3 changed files with 103 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

178
subtask.py
View File

@@ -187,7 +187,6 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
ret, box = filter.get(label)
while not ret:
ret, box = filter.get(label)
print(box)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
@@ -347,9 +346,7 @@ class get_block1():
# act.axis.exec()
by_cmd.send_position_axis_z(30, 60)
by_cmd.send_position_axis_x(1, 100)
time.sleep(1)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20)
time.sleep(2)
@@ -366,7 +363,7 @@ class get_block1():
by_cmd.send_position_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
time.sleep(1)
by_cmd.send_angle_claw(45)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 80)
@@ -407,10 +404,10 @@ class get_block2():
time.sleep(0.5)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw(53)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
by_cmd.send_position_axis_z(30, 60)
time.sleep(1)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
by_cmd.send_angle_claw(30)
by_cmd.send_beep(1)
time.sleep(0.5)
@@ -465,6 +462,8 @@ class put_block():
time.sleep(2)
by_cmd.send_angle_claw_arm(36)
time.sleep(1)
by_cmd.send_angle_claw(45)
time.sleep(1)
by_cmd.send_position_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
@@ -472,8 +471,9 @@ class put_block():
by_cmd.send_angle_claw(30)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 140)
time.sleep(3)
time.sleep(2)
by_cmd.send_angle_claw_arm(220)
time.sleep(1)
by_cmd.send_distance_x(-10, 110)
by_cmd.send_position_axis_z(30, 0)
time.sleep(2)
@@ -489,11 +489,13 @@ class put_block():
def after(self):
var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
# 下一动作预备位置
by_cmd.send_position_axis_z(30, 130)
while by_cmd.send_position_axis_z(30, 135) == -1:
pass
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
by_cmd.send_angle_claw_arm(36)
while by_cmd.send_position_axis_x(1, 0) == -1:
pass
while by_cmd.send_angle_claw_arm(36) == -1:
pass
# 任务检查间隔
# time.sleep(2)
@@ -503,11 +505,11 @@ class get_bball():
def init(self):
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
by_cmd.send_position_axis_z(30, 135)
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_position_axis_z(30, 135)
# time.sleep(0.5)
# by_cmd.send_position_axis_x(1, 0)
# time.sleep(2)
# by_cmd.send_angle_claw_arm(36)
while (by_cmd.send_angle_storage(0) == -1):
by_cmd.send_angle_storage(0)
@@ -520,7 +522,8 @@ class get_bball():
self.record = CountRecord(5)
def find(self):
# 目标检测蓝球
ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
ret = filter.find(tlabel.BBALL)
# ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
# TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
# if ret:
# if self.record(tlabel.BBALL):
@@ -546,12 +549,12 @@ class get_bball():
time.sleep(1)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_position_axis_x(1, 40)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, -40)
time.sleep(0.5)
by_cmd.send_angle_claw_arm(70)
time.sleep(0.5)
by_cmd.send_distance_axis_z(30, -40)
time.sleep(1)
by_cmd.send_angle_claw(54)
time.sleep(1)
by_cmd.send_angle_claw_arm(36)
@@ -566,8 +569,9 @@ class get_bball():
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
# 下一动作预备位置
by_cmd.send_angle_claw(30)
time.sleep(0.5)
by_cmd.send_position_axis_z(30, 0)
time.sleep(2)
time.sleep(1)
# # 任务检查间隔
# time.sleep(1)
@@ -589,16 +593,16 @@ class up_tower():
logger.info("找到塔")
car_stop()
time.sleep(1)
calibrate_new(tlabel.TOWER, offset = 27, run = True)
calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 130)
by_cmd.send_distance_x(-10, 120)
time.sleep(1)
by_cmd.send_distance_y(-10, 50)
time.sleep(3)
by_cmd.send_angle_zhuan(10)
time.sleep(9)
by_cmd.send_distance_y(10, 50)
by_cmd.send_distance_y(10, 60)
time.sleep(1)
by_cmd.send_angle_zhuan(0)
time.sleep(0.5)
@@ -658,7 +662,7 @@ class get_rball():
def nexec(self):
pass
def after(self):
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
var.pid_turning.set(cfg["get_rball"]["pid_kp"], cfg["get_rball"]["pid_ki"], cfg["get_rball"]["pid_kd"])
# 任务检查间隔
time.sleep(2)
@@ -680,15 +684,16 @@ class put_bball():
def exec(self):
logger.info("找到篮筐")
car_stop()
calibrate_new(tlabel.BASKET,offset = -15, run = True, run_speed = 5.5)
by_cmd.send_distance_y(-10, 65)
calibrate_new(tlabel.BASKET,offset = -21, run = True, run_speed = 5.5)
by_cmd.send_distance_x(10, 20)
by_cmd.send_distance_y(-10, 55)
time.sleep(1)
by_cmd.send_angle_storage(55)
logger.info("把球放篮筐里")
time.sleep(2)
by_cmd.send_distance_y(10, 65)
by_cmd.send_distance_y(10, 55)
time.sleep(1)
by_cmd.send_angle_storage(20)
@@ -794,6 +799,7 @@ class put_hanoi1():
def nexec(self):
pass
def after(self):
var.pid_turning.set(0.8, 0, 0)
pass
class put_hanoi2():
@@ -841,13 +847,13 @@ class put_hanoi2():
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(2)
time.sleep(1.5)
by_cmd.send_angle_claw(45)
time.sleep(2)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(2, 10)
time.sleep(4)
by_cmd.send_position_axis_x(1, 10)
time.sleep(2)
pass
else:
by_cmd.send_position_axis_z(30, 10)
@@ -855,20 +861,20 @@ class put_hanoi2():
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(2)
time.sleep(1.5)
by_cmd.send_angle_claw(45)
time.sleep(2)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
by_cmd.send_position_axis_x(1, 160)
time.sleep(2)
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
logger.info("放大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_x(2, 130)
time.sleep(4)
by_cmd.send_position_axis_x(1, 130)
time.sleep(1.5)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
@@ -876,8 +882,8 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 10)
pass
else:
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
by_cmd.send_position_axis_x(1, 40)
time.sleep(1.5)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
@@ -898,8 +904,8 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(2, 10)
time.sleep(4)
by_cmd.send_position_axis_x(1, 10)
time.sleep(3)
pass
else:
by_cmd.send_position_axis_z(30, 10)
@@ -912,33 +918,35 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
by_cmd.send_position_axis_x(1, 160)
time.sleep(3)
pass
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
logger.info("放中平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 100)
time.sleep(4)
by_cmd.send_position_axis_x(2, 130)
time.sleep(4)
time.sleep(2)
by_cmd.send_position_axis_x(1, 130)
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
by_cmd.send_angle_claw(90)
time.sleep(1)
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)
time.sleep(4)
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
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(90)
time.sleep(1)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
time.sleep(1)
@@ -949,13 +957,13 @@ class put_hanoi2():
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(2)
time.sleep(1)
by_cmd.send_angle_claw(30)
time.sleep(2)
time.sleep(1)
by_cmd.send_distance_axis_z(30, 10)
time.sleep(1)
by_cmd.send_position_axis_x(2, 10)
time.sleep(4)
by_cmd.send_position_axis_x(1, 10)
time.sleep(2)
pass
else:
by_cmd.send_position_axis_z(30, 10)
@@ -963,40 +971,40 @@ class put_hanoi2():
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(2)
time.sleep(1)
by_cmd.send_angle_claw(30)
time.sleep(2)
time.sleep(1)
by_cmd.send_distance_axis_z(30, 10)
time.sleep(1)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
by_cmd.send_position_axis_x(1, 160)
time.sleep(2)
pass
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
logger.info("放小平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 170)
time.sleep(4)
by_cmd.send_position_axis_x(2, 130)
time.sleep(4)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1.5)
by_cmd.send_position_axis_x(1, 130)
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
by_cmd.send_angle_claw(90)
time.sleep(1)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1.5)
pass
else:
by_cmd.send_position_axis_z(30, 170)
time.sleep(4)
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1.5)
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(90)
time.sleep(1)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1.5)
# while True:
# pass
@@ -1007,14 +1015,18 @@ class put_hanoi2():
class put_hanoi3():
def init(self):
by_cmd.send_position_axis_z(30, 130)
while by_cmd.send_position_axis_z(30, 130) == -1:
pass
time.sleep(3)
logger.info("完成任务，爪回左侧")
by_cmd.send_angle_claw_arm(128)
while by_cmd.send_angle_claw_arm(128) == -1:
pass
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 150)
while by_cmd.send_position_axis_x(1, 150) == -1:
pass
time.sleep(1)
by_cmd.send_angle_claw_arm(220)
while by_cmd.send_angle_claw_arm(220) == -1:
pass
def find(self):
time.sleep(1)
return True
@@ -1072,6 +1084,8 @@ class move_area2():
time.sleep(1)
# 进入停车区域
by_cmd.send_distance_y(10, 450)
time.sleep(3)
# TODO 调用大模型 然后执行动作
@@ -1098,6 +1112,10 @@ class kick_ass():
self.pos_gap1 = cfg['kick_ass']['pos_gap1']
self.pos_gap2 = cfg['kick_ass']['pos_gap2']
self.target_person = cfg['kick_ass']['target_person']
by_cmd.send_angle_claw(15)
by_cmd.send_position_axis_x(1, 160)
def find(self):
ret = filter.find(tlabel.SIGN)
if ret:
@@ -1123,9 +1141,9 @@ class kick_ass():
by_cmd.send_position_axis_x(1, 160)
time.sleep(2)
by_cmd.send_position_axis_x(1, 0)
time.sleep(4)
by_cmd.send_position_axis_x(1, 160)
time.sleep(3)
by_cmd.send_position_axis_x(1, 160)
time.sleep(2)
pass
def nexec(self):
pass