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

feat: 增加hanoi右侧动作

Browse Source
This commit is contained in:
bmy
2024-06-22 20:47:00 +08:00
parent e72f1e5c24
commit 90b888eac7
3 changed files with 113 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
main.py
View File

@@ -18,7 +18,7 @@ cfg_main = toml.load('cfg_main.toml')
# 配置日志输出
logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
act.axis.camera(180)
act.axis.camera(0)
act.axis.x2(140)
act.axis.storage(20)
act.axis.scoop(25)

1
majtask.py
View File

@@ -99,5 +99,4 @@ class main_task():
self.socket.send_string("")
resp = self.socket.recv_pyobj()
# logger.info(resp)
self.parse_data(resp)

206
subtask.py
View File

@@ -305,9 +305,10 @@ class task_queuem(task):
# 人员施救
class get_block1():
def init(self):
var.task_speed = 12
var.task_speed = 0
logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
act.cmd.camera(0)
act.cmd.z2(20, 60, 0)
filter.switch_camera(1)
if cfg['get_block']['first_block'] == "blue":
self.target_label = tlabel.BBLOCK
@@ -345,7 +346,7 @@ class get_block1():
# act.axis.x2(100)
# act.axis.exec()
by_cmd.send_position_axis_z(20, 60)
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)
@@ -358,17 +359,17 @@ class get_block1():
by_cmd.send_beep(0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 80)
by_cmd.send_position_axis_z(20, 130)
by_cmd.send_position_axis_z(30, 130)
time.sleep(2)
by_cmd.send_angle_claw_arm(36)
time.sleep(1)
by_cmd.send_position_axis_z(20, 20)
by_cmd.send_position_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
by_cmd.send_angle_claw(30)
by_cmd.send_angle_claw(45)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 80)
by_cmd.send_position_axis_z(30, 80)
time.sleep(2)
by_cmd.send_angle_claw_arm(220)
time.sleep(0.5)
@@ -405,10 +406,10 @@ class get_block2():
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw(63)
by_cmd.send_angle_claw(53)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
by_cmd.send_position_axis_z(20, 60)
by_cmd.send_position_axis_z(30, 60)
time.sleep(1)
by_cmd.send_angle_claw(30)
by_cmd.send_beep(1)
@@ -453,32 +454,32 @@ class put_block():
by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(20, 0)
by_cmd.send_position_axis_z(30, 0)
time.sleep(3)
by_cmd.send_position_axis_x(1, 40)
time.sleep(1)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_position_axis_x(1, 80)
by_cmd.send_position_axis_z(20, 130)
by_cmd.send_position_axis_z(30, 130)
time.sleep(2)
by_cmd.send_angle_claw_arm(36)
time.sleep(1)
by_cmd.send_position_axis_z(20, 20)
by_cmd.send_position_axis_z(30, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
by_cmd.send_angle_claw(30)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 140)
by_cmd.send_position_axis_z(30, 140)
time.sleep(3)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_distance_x(-10, 110)
by_cmd.send_position_axis_z(20, 0)
by_cmd.send_position_axis_z(30, 0)
time.sleep(2)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
by_cmd.send_angle_claw(63)
by_cmd.send_angle_claw(45)
time.sleep(1)
by_cmd.send_position_axis_x(1, 80)
time.sleep(1)
@@ -488,10 +489,11 @@ 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(20, 130)
by_cmd.send_position_axis_z(30, 130)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
by_cmd.send_angle_claw_arm(36)
# 任务检查间隔
# time.sleep(2)
@@ -501,7 +503,7 @@ 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(20, 135)
by_cmd.send_position_axis_z(30, 135)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 0)
time.sleep(2)
@@ -519,11 +521,12 @@ class get_bball():
def find(self):
# 目标检测蓝球
ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
if ret:
if self.record(tlabel.BBALL):
return True
# TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
# if ret:
# return True
# if self.record(tlabel.BBALL):
# return True
if ret:
return True
return False
def exec(self):
logger.info("找到蓝色球")
@@ -541,18 +544,19 @@ class get_bball():
time.sleep(1)
by_cmd.send_angle_claw(30)
time.sleep(1)
by_cmd.send_distance_axis_z(20, 20)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5)
by_cmd.send_angle_claw_arm(60)
by_cmd.send_angle_claw_arm(70)
time.sleep(0.5)
by_cmd.send_distance_axis_z(20, -20)
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)
time.sleep(1)
by_cmd.send_position_axis_z(30, 135)
# 继续向前走
# by_cmd.send_speed_x(4)
pass
@@ -562,7 +566,7 @@ 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)
by_cmd.send_position_axis_z(20, 0)
by_cmd.send_position_axis_z(30, 0)
time.sleep(2)
# # 任务检查间隔
# time.sleep(1)
@@ -605,7 +609,7 @@ class up_tower():
def after(self):
var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 0)
by_cmd.send_position_axis_z(30, 0)
# 任务检查间隔
time.sleep(4)
@@ -642,7 +646,7 @@ class get_rball():
logger.info("抓红球")
# by_cmd.send_angle_scoop(15)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 170)
by_cmd.send_position_axis_z(30, 170)
time.sleep(5)
by_cmd.send_angle_scoop(7)
by_cmd.send_distance_y(15, 70)
@@ -664,7 +668,7 @@ class put_bball():
def init(self):
logger.info("派发物资初始化")
filter.switch_camera(1)
by_cmd.send_position_axis_z(20, 0)
by_cmd.send_position_axis_z(30, 0)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
def find(self):
@@ -726,7 +730,7 @@ class put_hanoi1():
by_cmd.send_speed_omega(0)
time.sleep(0.2)
by_cmd.send_position_axis_z(20, 130)
by_cmd.send_position_axis_z(30, 130)
# 校准牌子
if utils.direction_right > utils.direction_left:
@@ -767,7 +771,7 @@ class put_hanoi1():
by_cmd.send_angle_storage(55)
time.sleep(1.5)
by_cmd.send_position_axis_z(20, 0)
by_cmd.send_position_axis_z(30, 0)
if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK
@@ -832,20 +836,21 @@ class put_hanoi2():
time.sleep(1)
logger.info("抓大平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40)
# by_cmd.send_position_axis_x(1, 160)
# by_cmd.send_angle_claw(63)
# time.sleep(1)
# by_cmd.send_angle_claw(85)
# time.sleep(4)
# by_cmd.send_angle_claw(50)
# time.sleep(4)
# by_cmd.send_position_axis_x(2, 80)
# # FIXME 多往回收一些 会挡住识别
# by_cmd.send_distance_axis_z(20, 10)
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(2)
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)
pass
else:
by_cmd.send_position_axis_z(20, 10)
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
@@ -853,7 +858,7 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_angle_claw(45)
time.sleep(2)
by_cmd.send_distance_axis_z(20, 20)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
@@ -862,16 +867,18 @@ class put_hanoi2():
time.sleep(1)
logger.info("放大平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10)
# by_cmd.send_position_axis_x(2, 160)
# time.sleep(4)
# by_cmd.send_angle_claw(90)
# by_cmd.send_position_axis_x(1, 0)
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(1)
by_cmd.send_position_axis_x(1, 10)
pass
else:
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
by_cmd.send_distance_axis_z(20, -20)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(1)
@@ -881,20 +888,21 @@ class put_hanoi2():
time.sleep(1)
logger.info("抓中平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40)
# by_cmd.send_position_axis_x(1, 160)
# by_cmd.send_angle_claw(63)
# time.sleep(1)
# by_cmd.send_angle_claw(85)
# time.sleep(4)
# by_cmd.send_angle_claw(37)
# time.sleep(4)
# by_cmd.send_position_axis_x(2, 40)
# # FIXME 多往回收一些 会挡住识别
# by_cmd.send_distance_axis_z(20, 10)
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(2)
by_cmd.send_angle_claw(40)
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)
pass
else:
by_cmd.send_position_axis_z(20, 10)
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
@@ -902,7 +910,7 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_angle_claw(40)
time.sleep(2)
by_cmd.send_distance_axis_z(20, 20)
by_cmd.send_distance_axis_z(30, 20)
time.sleep(1)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
@@ -911,19 +919,23 @@ class put_hanoi2():
time.sleep(1)
logger.info("放中平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10)
# by_cmd.send_position_axis_x(2, 160)
# time.sleep(4)
# by_cmd.send_angle_claw(90)
# by_cmd.send_position_axis_x(1, 0)
by_cmd.send_position_axis_z(30, 100)
time.sleep(4)
by_cmd.send_position_axis_x(2, 130)
time.sleep(4)
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)
pass
else:
by_cmd.send_position_axis_z(20, 100)
by_cmd.send_position_axis_z(30, 100)
time.sleep(4)
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(1)
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)
@@ -932,20 +944,21 @@ class put_hanoi2():
time.sleep(1)
logger.info("抓小平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40)
# by_cmd.send_position_axis_x(1, 160)
# by_cmd.send_angle_claw(63)
# time.sleep(1)
# by_cmd.send_angle_claw(85)
# time.sleep(4)
# by_cmd.send_angle_claw(37)
# time.sleep(4)
# by_cmd.send_position_axis_x(2, 40)
# # FIXME 多往回收一些 会挡住识别
# by_cmd.send_distance_axis_z(20, 10)
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(2)
by_cmd.send_angle_claw(30)
time.sleep(2)
by_cmd.send_distance_axis_z(30, 10)
time.sleep(1)
by_cmd.send_position_axis_x(2, 10)
time.sleep(4)
pass
else:
by_cmd.send_position_axis_z(20, 10)
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
@@ -953,7 +966,7 @@ class put_hanoi2():
time.sleep(2)
by_cmd.send_angle_claw(30)
time.sleep(2)
by_cmd.send_distance_axis_z(20, 10)
by_cmd.send_distance_axis_z(30, 10)
time.sleep(1)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
@@ -962,21 +975,26 @@ class put_hanoi2():
time.sleep(1)
logger.info("放小平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10)
# by_cmd.send_position_axis_x(2, 160)
# time.sleep(4)
# by_cmd.send_angle_claw(90)
# by_cmd.send_position_axis_x(1, 0)
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(2)
pass
else:
by_cmd.send_position_axis_z(20, 160)
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(20, -20)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(1)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 160)
time.sleep(2)
# while True:
@@ -989,7 +1007,7 @@ class put_hanoi2():
class put_hanoi3():
def init(self):
by_cmd.send_position_axis_z(20, 130)
by_cmd.send_position_axis_z(30, 130)
time.sleep(3)
logger.info("完成任务，爪回左侧")
by_cmd.send_angle_claw_arm(128)
@@ -1001,7 +1019,7 @@ class put_hanoi3():
time.sleep(1)
return True
def exec(self):
by_cmd.send_position_axis_z(20, 100)
by_cmd.send_position_axis_z(30, 100)
pass
def nexec(self):
pass
@@ -1068,7 +1086,7 @@ class move_area2():
pass
def after(self):
var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
by_cmd.send_position_axis_z(20, 0)
by_cmd.send_position_axis_z(30, 0)
time.sleep(2)
# 扫黑除暴
@@ -1092,7 +1110,7 @@ class kick_ass():
time.sleep(1)
calibrate_new(tlabel.SIGN, offset = 8, run = True)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 60)
by_cmd.send_position_axis_z(30, 60)
if self.target_person == 1:
target_distance = self.pos_gap1