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

pref: 优化动作后处理

Browse Source
This commit is contained in:
bmy
2024-06-09 16:48:31 +08:00
parent eb0b01c8af
commit 188127b419
2 changed files with 98 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
main.py
View File

@@ -18,18 +18,19 @@ logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logge
# 向任务队列添加任务 # 向任务队列添加任务
task_queue = queue.Queue() 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.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.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'])) # TODO 添加一个空任务用于提前降 z 轴
# 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.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_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_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.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_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.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.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable']))
# 将任务队列传入调度模块中 # 将任务队列传入调度模块中
task_queuem_t = sb.task_queuem(task_queue) task_queuem_t = sb.task_queuem(task_queue)
@@ -44,7 +45,11 @@ worker = threading.Thread(target=worker_thread, daemon=True)
worker.start() worker.start()
# if (cmd_py_obj.send_angle_camera(180) == -1): # if (cmd_py_obj.send_angle_camera(180) == -1):
# cmd_py_obj.send_angle_camera(180) # 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) cmd_py_obj.send_angle_storage(20)
time.sleep(0.5)
cmd_py_obj.send_angle_scoop(25)
time.sleep(2) time.sleep(2)
# 创建主任务 # 创建主任务

130
subtask.py
View File

@@ -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 error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret: if ret:
if abs(error) <= 5: if abs(error) <= 10: # 5
car_stop() car_stop()
logger.info("可以停车了") logger.info("可以停车了")
ret, box = filter.get(label) ret, box = filter.get(label)
while not ret: while not ret:
ret, box = filter.get(label) ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
if abs(error) > 8: if abs(error) > 8:
error = error * 3 error = error * 1.5 # 3
if error > 0: if error > 0:
by_cmd.send_distance_x(-10, int(error)) by_cmd.send_distance_x(-10, int(error))
else: else:
@@ -239,6 +241,8 @@ class task_queuem(task):
class get_block(): class get_block():
def init(self): def init(self):
logger.info("人员施救初始化") logger.info("人员施救初始化")
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
filter.switch_camera(1) filter.switch_camera(1)
if cfg['get_block']['first_block'] == "blue": if cfg['get_block']['first_block'] == "blue":
self.target_label = tlabel.BBLOCK self.target_label = tlabel.BBLOCK
@@ -255,8 +259,24 @@ class get_block():
return False return False
def exec(self): def exec(self):
car_stop() car_stop()
calibrate_new(self.target_label, offset = 12) calibrate_new(self.target_label, offset = 12, run = True)
logger.info("抓取块") 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) time.sleep(3)
pass pass
@@ -298,6 +318,8 @@ class get_block():
# 紧急转移 # 紧急转移
class put_block(): class put_block():
def init(self): def init(self):
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
logger.info("紧急转移初始化") logger.info("紧急转移初始化")
socket.send_string("1") socket.send_string("1")
socket.recv() socket.recv()
@@ -313,17 +335,35 @@ class put_block():
logger.info("找到医院") logger.info("找到医院")
car_stop() car_stop()
calibrate_new(tlabel.HOSPITAL, offset = 7, run = False) 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) # by_cmd.send_position_axis_z(10, 150)
# time.sleep(3) # time.sleep(3)
# # TODO 切换爪子方向 # # TODO 切换爪子方向
# by_cmd.send_position_axis_x(2, 140) # by_cmd.send_position_axis_x(2, 140)
# time.sleep(2) # time.sleep(2)
# by_cmd.send_position_axis_z(10, 170) # 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 pass
def nexec(self): 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 pass
# 整装上阵 # 整装上阵
@@ -331,7 +371,7 @@ class get_bball():
def init(self): def init(self):
while (by_cmd.send_angle_camera(90) == -1): while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90) 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) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 0) by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5) time.sleep(0.5)
@@ -341,7 +381,7 @@ class get_bball():
socket.send_string("1") socket.send_string("1")
socket.recv() socket.recv()
logger.info("整装上阵初始化") logger.info("整装上阵初始化")
time.sleep(0.5) # time.sleep(0.5)
def find(self): def find(self):
# 目标检测蓝球 # 目标检测蓝球
@@ -354,54 +394,40 @@ class get_bball():
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
for _ in range(3): 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("抓蓝色球") logger.info("抓蓝色球")
by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_claw(54) 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) time.sleep(1)
by_cmd.send_angle_claw(70) by_cmd.send_angle_claw(70)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(30) by_cmd.send_angle_claw(30)
time.sleep(1) time.sleep(1)
by_cmd.send_distance_axis_z(10, 20) by_cmd.send_distance_axis_z(20, 20)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 0) by_cmd.send_position_axis_x(1, 0)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw_arm(82) by_cmd.send_angle_claw_arm(67)
time.sleep(1) time.sleep(0.5)
by_cmd.send_angle_claw(54) by_cmd.send_angle_claw(54)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw_arm(36)
time.sleep(1) time.sleep(1)
by_cmd.send_distance_axis_z(10, -20) by_cmd.send_distance_axis_z(20, -20)
time.sleep(1) time.sleep(1)
# 下一动作预备位置
by_cmd.send_angle_claw(30)
# by_cmd.send_position_axis_z(20, 160) by_cmd.send_position_axis_z(20, 0)
# time.sleep(2) 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)
pass pass
def nexec(self): def nexec(self):
by_cmd.send_angle_claw(30)
by_cmd.send_positioin_axis_z(20, 0)
time.sleep(2)
pass pass
# 通信抢修 # 通信抢修
@@ -420,13 +446,19 @@ class up_tower():
logger.info("找到塔") logger.info("找到塔")
car_stop() car_stop()
calibrate_new(tlabel.TOWER, offset = 27, run = True) calibrate_new(tlabel.TOWER, offset = 27, run = True)
by_cmd.send_light(1)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_light(0)
# calibrate(tlabel.TOWER, 27, False, 6) # calibrate(tlabel.TOWER, 27, False, 6)
time.sleep(3) time.sleep(3)
while True: # while True:
pass # pass
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 0)
def nexec(self): def nexec(self):
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 0)
time.sleep(4)
pass pass
@@ -440,21 +472,31 @@ class get_rball():
# 目标检测红球 # 目标检测红球
ret = filter.find(tlabel.RBALL) ret = filter.find(tlabel.RBALL)
if ret > 0: if ret > 0:
utils.task_speed = 5
return True return True
else: else:
return False return False
def exec(self): def exec(self):
logger.info("找到红球") logger.info("找到红球")
utils.task_speed = 0
car_stop() car_stop()
calibrate_new(tlabel.RBALL,offset = -4, run = True) time.sleep(0.5)
time.sleep(1) # 靠近塔
by_cmd.send_angle_scoop(20)
by_cmd.send_distance_y(-10, 65) by_cmd.send_distance_y(-10, 65)
time.sleep(1) time.sleep(1)
by_cmd.send_distance_x(-10, 80) calibrate_new(tlabel.RBALL,offset = 50, run = True)
time.sleep(1) time.sleep(1)
logger.info("抓红球") logger.info("抓红球")
while True: # by_cmd.send_angle_scoop(15)
pass 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 pass
def nexec(self): def nexec(self):
pass pass