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

2
cfg_main.toml
View File

@@ -15,7 +15,7 @@ MoveArea_enable = true # 应急避险使能
KickAss_enable = true # 扫黑除暴使能 KickAss_enable = true # 扫黑除暴使能
[find_counts] [find_counts]
GetBlock_counts = 10 # 人员施救计数 GetBlock_counts = 5 # 人员施救计数
PutBlock_counts = 5 # 紧急转移计数 PutBlock_counts = 5 # 紧急转移计数
GetBBall_counts = 1 # 整装上阵计数 GetBBall_counts = 1 # 整装上阵计数
UpTower_counts = 3 # 通信抢修计数 UpTower_counts = 3 # 通信抢修计数

8
cfg_subtask.toml
View File

@@ -6,8 +6,8 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
# *第一个抓取的方块 # *第一个抓取的方块
first_block = "red" # first_block = "red"
# first_block = "blue" first_block = "blue"
################################################ ################################################
[put_block] [put_block]
@@ -33,7 +33,7 @@ pid_kd = 0
################################################ ################################################
[get_rball] [get_rball]
# pid 参数值 # pid 参数值
pid_kp = 0.4 pid_kp = 0.8
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
@@ -88,5 +88,5 @@ pid_kd = 0
pos_gap1 = 150 # 目标牌和第一个 person 之间的距离 pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
pos_gap2 = 80 # person 之间的距离 pos_gap2 = 80 # person 之间的距离
target_person = 4 # 击打的人 - 最靠近标识牌的为 1 target_person = 2 # 击打的人 - 最靠近标识牌的为 1
################################################ ################################################

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