From 188127b419cea145281e0b92ce0ef0e0b8a3b0a8 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Sun, 9 Jun 2024 16:48:31 +0800 Subject: [PATCH] =?UTF-8?q?pref:=20=E4=BC=98=E5=8C=96=E5=8A=A8=E4=BD=9C?= =?UTF-8?q?=E5=90=8E=E5=A4=84=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main.py | 19 +++++--- subtask.py | 130 +++++++++++++++++++++++++++++++++++------------------ 2 files changed, 98 insertions(+), 51 deletions(-) diff --git a/main.py b/main.py index c1cd341..50f649f 100644 --- a/main.py +++ b/main.py @@ -18,18 +18,19 @@ logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logge # 向任务队列添加任务 task_queue = queue.Queue() +task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) # task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) -# task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) -# task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'] and cfg_main['task']['PutBlock_enable'])) +task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'] and cfg_main['task']['PutBlock_enable'])) task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) -# task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable'])) -# task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable'])) +# TODO 添加一个空任务用于提前降 z 轴 +task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable'])) +task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable'])) task_queue.put(sb.task(sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_main['task']['GetBBall_enable'] and cfg_main['task']['PutBBall_enable'])) task_queue.put(sb.task(sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], True)) # 无论是否进行任务，检测标识并转向都是必须进行的 task_queue.put(sb.task(sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable'])) -# task_queue.put(sb.task(sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable'])) -# task_queue.put(sb.task(sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable'])) -# task_queue.put(sb.task(sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable'])) +task_queue.put(sb.task(sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable'])) +task_queue.put(sb.task(sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable'])) +task_queue.put(sb.task(sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable'])) # 将任务队列传入调度模块中 task_queuem_t = sb.task_queuem(task_queue) @@ -44,7 +45,11 @@ worker = threading.Thread(target=worker_thread, daemon=True) worker.start() # if (cmd_py_obj.send_angle_camera(180) == -1): # cmd_py_obj.send_angle_camera(180) +cmd_py_obj.send_distance_axis_x(1, 140) +time.sleep(0.5) cmd_py_obj.send_angle_storage(20) +time.sleep(0.5) +cmd_py_obj.send_angle_scoop(25) time.sleep(2) # 创建主任务 diff --git a/subtask.py b/subtask.py index 524b31a..e2de386 100644 --- a/subtask.py +++ b/subtask.py @@ -114,16 +114,18 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5): error = (box[0][2] + box[0][0] - 320) / 2 + offset if ret: - if abs(error) <= 5: + if abs(error) <= 10: # 5 car_stop() logger.info("可以停车了") ret, box = filter.get(label) while not ret: ret, box = filter.get(label) + error = (box[0][2] + box[0][0] - 320) / 2 + offset + logger.info(f"停车后像素误差:{error}") if abs(error) > 8: - error = error * 3 + error = error * 1.5 # 3 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: @@ -239,6 +241,8 @@ class task_queuem(task): class get_block(): def init(self): logger.info("人员施救初始化") + while (by_cmd.send_angle_camera(180) == -1): + by_cmd.send_angle_camera(180) filter.switch_camera(1) if cfg['get_block']['first_block'] == "blue": self.target_label = tlabel.BBLOCK @@ -255,8 +259,24 @@ class get_block(): return False def exec(self): car_stop() - calibrate_new(self.target_label, offset = 12) + calibrate_new(self.target_label, offset = 12, run = True) logger.info("抓取块") + time.sleep(0.5) + by_cmd.send_position_axis_z(15, 70) + by_cmd.send_position_axis_x(1, 140) + time.sleep(4) + by_cmd.send_angle_claw_arm(220) + by_cmd.send_angle_claw(63) + by_cmd.send_position_axis_x(1, 20) + time.sleep(2) + by_cmd.send_angle_claw(30) + by_cmd.send_light(1) + by_cmd.send_beep(1) + time.sleep(0.5) + by_cmd.send_light(0) + by_cmd.send_beep(0) + time.sleep(0.5) + by_cmd.send_position_axis_x(1, 140) time.sleep(3) pass @@ -298,6 +318,8 @@ class get_block(): # 紧急转移 class put_block(): def init(self): + while (by_cmd.send_angle_camera(180) == -1): + by_cmd.send_angle_camera(180) logger.info("紧急转移初始化") socket.send_string("1") socket.recv() @@ -313,17 +335,35 @@ class put_block(): logger.info("找到医院") car_stop() calibrate_new(tlabel.HOSPITAL, offset = 7, run = False) - time.sleep(3) - - + time.sleep(1) + by_cmd.send_position_axis_x(1, 0) + time.sleep(0.5) + by_cmd.send_position_axis_z(15, 0) + time.sleep(2) + by_cmd.send_angle_claw(63) + # time.sleep(2) + # by_cmd.send_position_axis_z(20, 130) + # while True: + # pass # by_cmd.send_position_axis_z(10, 150) # time.sleep(3) # # TODO 切换爪子方向 # by_cmd.send_position_axis_x(2, 140) # time.sleep(2) # by_cmd.send_position_axis_z(10, 170) + # 下一动作预备位置 + by_cmd.send_position_axis_z(20, 130) + time.sleep(3) + by_cmd.send_position_axis_x(1, 0) + by_cmd.send_angle_claw_arm(36) pass def nexec(self): + # 下一动作预备位置 + by_cmd.send_position_axis_z(20, 130) + time.sleep(1.5) + by_cmd.send_position_axis_x(1, 0) + by_cmd.send_angle_claw_arm(36) + time.sleep(4) pass # 整装上阵 @@ -331,7 +371,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(10, 130) + by_cmd.send_position_axis_z(20, 140) time.sleep(0.5) by_cmd.send_position_axis_x(1, 0) time.sleep(0.5) @@ -341,7 +381,7 @@ class get_bball(): socket.send_string("1") socket.recv() logger.info("整装上阵初始化") - time.sleep(0.5) + # time.sleep(0.5) def find(self): # 目标检测蓝球 @@ -354,54 +394,40 @@ class get_bball(): car_stop() time.sleep(0.5) for _ in range(3): - calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4) + explore_calibrate_new(tlabel.BBALL, offset = 27, run_direc = 1, run_speed = 4) + # calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4) logger.info("抓蓝色球") by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw(54) - by_cmd.send_position_axis_x(1, 140) + by_cmd.send_position_axis_x(1, 150) time.sleep(1) by_cmd.send_angle_claw(70) time.sleep(1) by_cmd.send_angle_claw(30) time.sleep(1) - by_cmd.send_distance_axis_z(10, 20) + by_cmd.send_distance_axis_z(20, 20) time.sleep(2) by_cmd.send_position_axis_x(1, 0) time.sleep(1) - by_cmd.send_angle_claw_arm(82) - time.sleep(1) + by_cmd.send_angle_claw_arm(67) + time.sleep(0.5) by_cmd.send_angle_claw(54) time.sleep(1) by_cmd.send_angle_claw_arm(36) time.sleep(1) - by_cmd.send_distance_axis_z(10, -20) - + by_cmd.send_distance_axis_z(20, -20) time.sleep(1) - - - # by_cmd.send_position_axis_z(20, 160) - # time.sleep(2) - # by_cmd.send_position_axis_x(2, 70) - # time.sleep(2) - # by_cmd.send_angle_claw(90) - # time.sleep(0.2) - # by_cmd.send_position_axis_x(2, 0) - # time.sleep(2) - # by_cmd.send_angle_claw(27) - # time.sleep(1) - # by_cmd.send_position_axis_z(20, 180) - # time.sleep(1) - # by_cmd.send_position_axis_x(4, 45) - # time.sleep(1) - # by_cmd.send_position_axis_z(20, 140) - # time.sleep(3) - # by_cmd.send_position_axis_x(2, 140) - # time.sleep(2) - # by_cmd.send_angle_claw(90) + # 下一动作预备位置 + by_cmd.send_angle_claw(30) + by_cmd.send_position_axis_z(20, 0) + time.sleep(2) pass def nexec(self): + by_cmd.send_angle_claw(30) + by_cmd.send_positioin_axis_z(20, 0) + time.sleep(2) pass # 通信抢修 @@ -420,13 +446,19 @@ class up_tower(): logger.info("找到塔") car_stop() calibrate_new(tlabel.TOWER, offset = 27, run = True) + by_cmd.send_light(1) time.sleep(0.5) - + by_cmd.send_light(0) # calibrate(tlabel.TOWER, 27, False, 6) time.sleep(3) - while True: - pass + # while True: + # pass + # 下一动作预备位置 + by_cmd.send_position_axis_z(20, 0) def nexec(self): + # 下一动作预备位置 + by_cmd.send_position_axis_z(20, 0) + time.sleep(4) pass @@ -440,21 +472,31 @@ class get_rball(): # 目标检测红球 ret = filter.find(tlabel.RBALL) if ret > 0: + utils.task_speed = 5 return True else: return False def exec(self): logger.info("找到红球") + utils.task_speed = 0 car_stop() - calibrate_new(tlabel.RBALL,offset = -4, run = True) - time.sleep(1) + time.sleep(0.5) + # 靠近塔 + by_cmd.send_angle_scoop(20) by_cmd.send_distance_y(-10, 65) time.sleep(1) - by_cmd.send_distance_x(-10, 80) + calibrate_new(tlabel.RBALL,offset = 50, run = True) time.sleep(1) logger.info("抓红球") - while True: - pass + # by_cmd.send_angle_scoop(15) + time.sleep(0.5) + by_cmd.send_position_axis_z(15, 170) + time.sleep(5) + by_cmd.send_angle_scoop(7) + by_cmd.send_distance_y(10, 65) + time.sleep(0.5) + # while True: + # pass pass def nexec(self): pass