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

update: 抓蓝块、转塔、应急避险和扫黑除恶动作修改

Browse Source
This commit is contained in:
bmy
2024-07-04 17:47:12 +08:00
parent c968e28b1f
commit f3e6bcc01f
1 changed files with 143 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

243
subtask.py
View File

@@ -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)