From f3e6bcc01f0e0ad5eb23cafb23f8fc74121abae4 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Thu, 4 Jul 2024 17:47:12 +0800 Subject: [PATCH] =?UTF-8?q?update:=20=E6=8A=93=E8=93=9D=E5=9D=97=E3=80=81?= =?UTF-8?q?=E8=BD=AC=E5=A1=94=E3=80=81=E5=BA=94=E6=80=A5=E9=81=BF=E9=99=A9?= =?UTF-8?q?=E5=92=8C=E6=89=AB=E9=BB=91=E9=99=A4=E6=81=B6=E5=8A=A8=E4=BD=9C?= =?UTF-8?q?=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- subtask.py | 243 +++++++++++++++++++++++++++++++---------------------- 1 file changed, 143 insertions(+), 100 deletions(-) diff --git a/subtask.py b/subtask.py index e16da13..668597f 100644 --- a/subtask.py +++ b/subtask.py @@ -1,8 +1,7 @@ ''' 待办事项： -- nexec 中添加延时，保证重试时不会立即跳入下个任务 -- 确认 nexec 还是直接添加一个任务结束后执行的方法更好，或者两者都保留 -- 医院第二个方块自由落体 +- 医院第二个方块动作适配 +- find 超时 ''' from enum import Enum from loguru import logger @@ -45,6 +44,8 @@ def car_stop(): for _ in range(3): by_cmd.send_speed_x(0) time.sleep(0.2) + by_cmd.send_speed_y(0) + time.sleep(0.2) by_cmd.send_speed_omega(0) # 蓝色球使用 def calibrate_right_new(label, offset, run = True, run_speed = 3.5): @@ -326,52 +327,33 @@ class get_block1(): car_stop() calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5) logger.info("抓取块") - # act.axis.wait(0.5) - # act.axis.z2(60) - # act.axis.x2(100) - # act.axis.claw_arm(220) - # act.axis.claw(63) - # act.axis.x2(20) - # act.axis.wait(2) - # act.axis.claw(30) - # act.axis.x2(80) - # act.axis.z2(130) - # act.axis.claw_arm(36) - # act.axis.z2(20) - # act.axis.x2(40) - # act.axis.claw(45) - # act.axis.z2(80) - # act.axis.claw_arm(220) - # act.axis.x2(100) - # act.axis.exec() by_cmd.send_position_axis_z(30, 60) time.sleep(1) by_cmd.send_angle_claw(63) by_cmd.send_position_axis_x(1, 20) - time.sleep(2) + time.sleep(1) by_cmd.send_angle_claw(25) - by_cmd.send_beep(1) - time.sleep(0.5) - by_cmd.send_beep(0) - time.sleep(0.5) - by_cmd.send_position_axis_x(1, 80) - 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(30, 20) - time.sleep(0.5) - by_cmd.send_position_axis_x(1, 40) - time.sleep(1) - by_cmd.send_angle_claw(45) time.sleep(0.5) by_cmd.send_position_axis_z(30, 80) - time.sleep(2) + time.sleep(0.5) + + by_cmd.send_angle_claw_arm(175) + time.sleep(0.1) + by_cmd.send_position_axis_x(1, 110) + time.sleep(1) + by_cmd.send_position_axis_z(30, 70) + time.sleep(0.1) + by_cmd.send_angle_claw(63) + time.sleep(0.5) + + by_cmd.send_position_axis_z(30, 130) + time.sleep(1) + by_cmd.send_position_axis_x(1, 140) by_cmd.send_angle_claw_arm(220) time.sleep(0.5) - by_cmd.send_position_axis_x(1, 100) - time.sleep(1) + by_cmd.send_angle_storage(55) + pass def nexec(self): # TODO 完成不执行任务的空动作 @@ -450,39 +432,28 @@ class put_block(): calibrate_new(tlabel.HOSPITAL, offset = 7, run = False) by_cmd.send_distance_x(10, 100) - by_cmd.send_position_axis_z(30, 0) - time.sleep(3) + time.sleep(0.5) 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(30, 130) - 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) - time.sleep(2) - by_cmd.send_angle_claw(25) - time.sleep(0.5) - by_cmd.send_position_axis_z(30, 140) - time.sleep(2) - by_cmd.send_angle_claw_arm(220) - time.sleep(1) + + # 放置第二個塊 + by_cmd.send_angle_storage(20) by_cmd.send_distance_x(-10, 110) - 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(45) time.sleep(1) - by_cmd.send_position_axis_x(1, 80) + by_cmd.send_position_axis_x(1, 150) + by_cmd.send_position_axis_z(30, 150) + time.sleep(2) + by_cmd.send_angle_claw_arm(126) + # by_cmd.send_angle_storage(0) time.sleep(1) + by_cmd.send_position_axis_z(15,100) + time.sleep(1) + by_cmd.send_position_axis_x(1, 0) + time.sleep(1.5) + by_cmd.send_angle_storage(0) pass def nexec(self): pass @@ -602,7 +573,7 @@ class up_tower(): by_cmd.send_distance_y(-10, 50) time.sleep(3) by_cmd.send_angle_zhuan(10) - time.sleep(9) + time.sleep(10) by_cmd.send_distance_y(10, 60) time.sleep(1) by_cmd.send_angle_zhuan(0) @@ -1014,6 +985,7 @@ class put_hanoi2(): def nexec(self): pass def after(self): + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) pass class put_hanoi3(): @@ -1025,11 +997,13 @@ class put_hanoi3(): while by_cmd.send_angle_claw_arm(128) == -1: pass time.sleep(0.5) - while by_cmd.send_position_axis_x(1, 150) == -1: + while by_cmd.send_position_axis_x(1, 0) == -1: pass time.sleep(1) - while by_cmd.send_angle_claw_arm(220) == -1: - pass + # while by_cmd.send_angle_claw_arm(220) == -1: + # pass + # while by_cmd.send_angle_claw(90) == -1: + # pass def find(self): time.sleep(1) return True @@ -1040,6 +1014,7 @@ class put_hanoi3(): def nexec(self): pass def after(self): + by_cmd.send_position_axis_x(1, 150) var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"]) # 应急避险 第一阶段 找目标牌 @@ -1057,14 +1032,55 @@ class move_area1(): logger.info("找到标示牌") # 停车 ocr 识别文字 调用大模型 car_stop() - time.sleep(2) - var.task_speed = 12 + time.sleep(1) + # calibrate_new(tlabel.SIGN, offset = 8, run = True) + calibrate_new(tlabel.SIGN, offset = -30, run = True, run_speed = 5) + time.sleep(1) + + by_cmd.send_position_axis_x(1, 50) + time.sleep(1) + + # filter_w = (148, 560) + # filter_h = (165, 390) + + counts = 0 + while True: + ocr_socket.send_string("") + resp = ocr_socket.recv_pyobj() + var.llm_text = '' + counts += 1 + if resp.get('code') == 0: + for item in resp.get('content'): + + if item['probability']['average'] < 0.80: + continue + # box = item['location'] + # center_x = box['left'] + box['width'] / 2 + # center_y = box['top'] + box['height'] / 2 + # if center_x < filter_w[0] or center_x > filter_w[1] \ + # or center_y < filter_h[0] or center_y > filter_h[1]: + # continue + var.llm_text += item['words'] + break + if counts >= 2: + var.skip_llm_task_flag = True + return + logger.error(var.llm_text) + if len(var.llm_text) < 3: + var.skip_llm_task_flag = True + return + + var.task_speed = 9 # 12 + pass def nexec(self): pass def after(self): # 任务检查间隔 - time.sleep(2) + by_cmd.send_position_axis_x(1, 150) + # time.sleep(1) + by_cmd.send_angle_claw_arm(220) + pass # 应急避险 第二阶段 找停车区域 @@ -1077,6 +1093,8 @@ class move_area2(): self.delta_omage = 0 def find(self): # time.sleep(0.001) + if var.skip_llm_task_flag: + return 5000 ret, box = filter.get(tlabel.SHELTER) if ret: error = (box[0][2] + box[0][0] - 320) / 2 + self.offset @@ -1121,33 +1139,55 @@ class move_area2(): time.sleep(delay_time / 300 * 1.5) def exec(self): var.task_speed = 0 + if var.skip_llm_task_flag: + logger.error("ocr 识别出错 直接跳过改任务") + return logger.info("开始寻找停车区域") car_stop() calibrate_new(tlabel.SHELTER, offset = 15, run = True) time.sleep(0.5) - # 进入停车区域 - by_cmd.send_distance_y(15, 300) - - - # time.sleep(1) - # 调用大模型 然后执行动作 - resp_commands = eval(llm_bot.get_command_json("旋转 90 度，发声 1 秒，照亮 1 秒").replace("'''json","").replace("'''",'')) - for command in resp_commands: - logger.info(command) - if command['func'] == 'move': - self.sub_move(float(command['x']), float(command['y'])) - elif command['func'] == 'light': - self.sub_light(int(command['time'])) - elif command['func'] == 'beep': - self.sub_beep(int(command['time'])) - elif command['func'] == 'turn': - self.sub_turn(int(command['angle'])) - pass - time.sleep(0.5) + try: + resp = llm_bot.get_command_json(var.llm_text) + logger.info(resp) + except: + logger.error("大模型超时，跳过任务") + return + + + try: + resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0]) + if len(resp_commands) == 0: + return + # 进入停车区域 + # by_cmd.send_speed_y(15) + by_cmd.send_distance_y(15, 300) + time.sleep(2) + # time.sleep(1.25) + car_stop() + logger.info(resp_commands) + for command in resp_commands: + logger.info(command) + if command['func'] == 'move': + self.sub_move(float(command['x']), float(command['y'])) + elif command['func'] == 'light': + self.sub_light(int(command['time'])) + elif command['func'] == 'beep': + self.sub_beep(int(command['time'])) + elif command['func'] == 'turn': + self.sub_turn(int(command['angle'])) + pass + else: + continue + time.sleep(0.5) + except: + pass time.sleep(1) # 回到原位 + delay_time = int(abs(self.delta_omage) * 400 / 90) + if int(abs(self.delta_omage)) == 360: + delay_time = 0 if self.delta_omage < 0: # 左转 by_cmd.send_angle_omega(-55, delay_time) @@ -1163,12 +1203,9 @@ class move_area2(): # 离开停车区域 by_cmd.send_distance_y(-15, delay_time) - time.sleep(delay_time / 400 * 1.5) + time.sleep(delay_time * 5e-3) - for _ in range(3): - by_cmd.send_speed_x(0) - by_cmd.send_speed_y(0) - by_cmd.send_speed_omega(0) + car_stop() # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值 # by_cmd.send_distance_y(-15, 300) @@ -1178,6 +1215,8 @@ class move_area2(): 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(30, 0) + while by_cmd.send_angle_claw(90) == -1: + pass time.sleep(2) # 扫黑除暴 @@ -1190,7 +1229,7 @@ class kick_ass(): 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_angle_claw(15) by_cmd.send_position_axis_x(1, 160) def find(self): @@ -1212,10 +1251,14 @@ class kick_ass(): else: target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10 by_cmd.send_distance_x(10, target_distance) - time.sleep(5) - by_cmd.send_angle_claw_arm(220) - by_cmd.send_angle_claw(15) - by_cmd.send_position_axis_x(1, 160) + time.sleep(4) + + while by_cmd.send_angle_claw_arm(220) == -1: + pass + while by_cmd.send_angle_claw(15) == -1: + pass + while by_cmd.send_position_axis_x(1, 160) == -1: + pass time.sleep(2) by_cmd.send_position_axis_x(1, 0) time.sleep(3)