From 90b888eac7078401a3aa320d624832a6203eea61 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Sat, 22 Jun 2024 20:47:00 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=A2=9E=E5=8A=A0hanoi=E5=8F=B3?= =?UTF-8?q?=E4=BE=A7=E5=8A=A8=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main.py | 2 +- majtask.py | 1 - subtask.py | 206 +++++++++++++++++++++++++++++------------------------ 3 files changed, 113 insertions(+), 96 deletions(-) diff --git a/main.py b/main.py index b18244b..a194ad1 100644 --- a/main.py +++ b/main.py @@ -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) diff --git a/majtask.py b/majtask.py index 7c512f1..1efbcfe 100644 --- a/majtask.py +++ b/majtask.py @@ -99,5 +99,4 @@ class main_task(): self.socket.send_string("") resp = self.socket.recv_pyobj() - # logger.info(resp) self.parse_data(resp) diff --git a/subtask.py b/subtask.py index ed1a8ab..82b4c1f 100644 --- a/subtask.py +++ b/subtask.py @@ -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