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

feat: 增加任务后处理方法

Browse Source
This commit is contained in:
bmy
2024-06-16 14:41:20 +08:00
parent ac72d1e3e6
commit f5be160fd9
4 changed files with 119 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
cfg_main.toml
View File

@@ -3,6 +3,7 @@ logger_filename = "log/file_{time}.log"
logger_format = "{time} {level} {message}"
[task]
Subtask_enable = true # 子任务总使能 调试用！
GetBlock_enable = true # 人员施救使能
PutBlock_enable = true # 紧急转移使能
GetBBall_enable = true # 整装上阵使能

2
cfg_subtask.toml
View File

@@ -33,7 +33,7 @@ pid_kd = 0
################################################
[get_rball]
# pid 参数值
pid_kp = 0.6
pid_kp = 0.4
pid_ki = 0
pid_kd = 0

1
main.py
View File

@@ -20,6 +20,7 @@ logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logge
# 向任务队列添加任务
task_queue = queue.Queue()
if cfg_main['task']['Subtask_enable'] is True:
task_queue.put(sb.task(sb.get_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable']))
task_queue.put(sb.task(sb.get_block2, 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']))

141
subtask.py
View File

@@ -160,8 +160,8 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
by_cmd.send_distance_x(-10, int(error))
else:
by_cmd.send_distance_x(10, int(-error))
break
def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
# run_direc == 1 向前
if run_direc == 1:
@@ -198,6 +198,7 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
by_cmd.send_distance_x(10, int(-error))
break
# 任务类
class task:
def __init__(self, task_template, find_counts=10, enable=True):
@@ -207,32 +208,48 @@ class task:
self.counts = 0
self.find_counts = find_counts
def init(self):
if hasattr(self.task_t, 'init') and callable(getattr(self.task_t, 'init', None)):
self.task_t.init()
else:
logger.warning("[Task ]# 该任务没有 init 方法")
def find(self):
# 检查该任执行标志
if hasattr(self.task_t, 'find') and callable(getattr(self.task_t, 'find', None)):
# 检查该任务执行标志
while True:
ret = self.task_t.find()
self.counts += ret
if self.counts >= self.find_counts:
break
else:
logger.warning("[Task ]# 该任务没有 find 方法")
def exec(self):
# 根据标志位确定是否执行该任务
if self.enable is True:
logger.debug(f"[Task ]# Executing task")
if hasattr(self.task_t, 'exec') and callable(getattr(self.task_t, 'exec', None)):
logger.info(f"[Task ]# Executing task")
self.task_t.exec()
logger.debug(f"[Task ]# Task completed")
else:
logger.warning("[Task ]# 该任务没有 exec 方法")
else:
logger.warning(f"[Task ]# Skip task")
if hasattr(self.task_t, 'nexec') and callable(getattr(self.task_t, 'nexec', None)):
self.task_t.nexec()
else:
logger.warning("[Task ]# 该任务没有 nexec 方法")
def after(self):
if hasattr(self.task_t, 'after') and callable(getattr(self.task_t, 'after', None)):
self.task_t.after()
logger.debug(f"[Task ]# Task completed")
else:
logger.warning("[Task ]# 该任务没有 after 方法")
# 任务队列状态类
class task_queuem_status(Enum):
IDEL = 0
SEARCHING = 1
EXECUTING = 2
# 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
# 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
class task_queuem(task):
# task_now = task(None, False)
def __init__(self, queue):
@@ -268,13 +285,15 @@ class task_queuem(task):
if self.task_now.enable is True:
logger.info(f"[TaskM]# Start execute task function")
self.task_now.exec() # 执行当前任务函数
self.status = task_queuem_status.IDEL # 执行完成后为退出巡线阻塞
self.task_now.after() # 执行任务后处理
self.queue.task_done() # 弹出已执行的任务
self.status = task_queuem_status.IDEL #
logger.info(f"[TaskM]# <<<<----------------------")
else:
logger.info(f"[TaskM]# Start execute task function (nexec)")
self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位
self.task_now.exec() # 执行当前任务函数
self.task_now.after() # 执行任务后处理
self.queue.task_done() # 弹出已执行的任务
logger.info(f"[TaskM]# <<<<----------------------")
return True
@@ -338,6 +357,8 @@ class get_block1():
def nexec(self):
# TODO 完成不执行任务的空动作
pass
def after(self):
pass
class get_block2():
def init(self):
@@ -375,15 +396,17 @@ class get_block2():
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 100)
by_cmd.send_distance_x(15, 100)
# by_cmd.send_distance_x(15, 100)
time.sleep(2)
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass
def nexec(self):
# TODO 完成不执行任务的空动作
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass
def after(self):
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
# 任务检查间隔
time.sleep(9)
# 紧急转移
@@ -432,29 +455,26 @@ class put_block():
by_cmd.send_angle_claw_arm(220)
by_cmd.send_distance_x(-10, 110)
by_cmd.send_position_axis_z(20, 0)
time.sleep(1)
time.sleep(2)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
by_cmd.send_angle_claw(63)
time.sleep(3)
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 130)
time.sleep(4)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
by_cmd.send_angle_claw_arm(36)
var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
by_cmd.send_position_axis_x(1, 80)
time.sleep(1)
pass
def nexec(self):
pass
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)
time.sleep(1.5)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw_arm(36)
time.sleep(4)
var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
pass
time.sleep(1)
# 任务检查间隔
# time.sleep(2)
# 整装上阵
class get_bball():
@@ -464,7 +484,8 @@ class get_bball():
by_cmd.send_position_axis_z(20, 135)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5)
time.sleep(2)
by_cmd.send_angle_claw_arm(36)
while (by_cmd.send_angle_storage(0) == -1):
by_cmd.send_angle_storage(0)
@@ -515,18 +536,18 @@ class get_bball():
time.sleep(1)
# 继续向前走
# by_cmd.send_speed_x(4)
pass
def nexec(self):
pass
def after(self):
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)
time.sleep(2)
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
pass
def nexec(self):
by_cmd.send_angle_claw(30)
by_cmd.send_position_axis_z(20, 0)
time.sleep(2)
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
pass
# # 任务检查间隔
# time.sleep(1)
# 通信抢修
class up_tower():
@@ -560,15 +581,14 @@ class up_tower():
time.sleep(0.5)
# while True:
# pass
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 0)
var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
def nexec(self):
pass
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)
# 任务检查间隔
time.sleep(4)
var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
pass
# 高空排险
@@ -612,11 +632,14 @@ class get_rball():
by_cmd.send_angle_omega(-55,30)
# while True:
# pass
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass
def nexec(self):
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass
def after(self):
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
# 任务检查间隔
time.sleep(2)
# 派发物资
class put_bball():
@@ -648,12 +671,13 @@ class put_bball():
time.sleep(1)
by_cmd.send_angle_storage(20)
var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
pass
def nexec(self):
var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
pass
def after(self):
var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
# 任务检查间隔
time.sleep(2)
# 物资盘点
@@ -750,11 +774,11 @@ class put_hanoi1():
socket.recv()
def nexec(self):
pass
def after(self):
pass
class put_hanoi2():
def __init__(self):
self.pos_lp = cfg['put_hanoi2']['pos_lp']
self.pos_mp = cfg['put_hanoi2']['pos_mp']
self.pos_sp = 6 - self.pos_lp - self.pos_mp
@@ -782,9 +806,7 @@ class put_hanoi2():
if abs(error) < 40:
return True
return False
def exec(self):
logger.info(f"direction:{utils.direction.name}")
var.task_speed = 0
car_stop()
@@ -947,6 +969,8 @@ class put_hanoi2():
def nexec(self):
pass
def after(self):
pass
class put_hanoi3():
def init(self):
@@ -963,11 +987,11 @@ class put_hanoi3():
return True
def exec(self):
by_cmd.send_position_axis_z(20, 100)
var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
pass
def nexec(self):
var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
pass
def after(self):
var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
# 应急避险 第一阶段 找目标牌
class move_area1():
@@ -989,6 +1013,11 @@ class move_area1():
pass
def nexec(self):
pass
def after(self):
# 任务检查间隔
time.sleep(2)
pass
# 应急避险 第二阶段 找停车区域
class move_area2():
def init(self):
@@ -1018,14 +1047,13 @@ class move_area2():
by_cmd.send_light(0)
# 离开停车区域
by_cmd.send_distance_y(-10, 450)
time.sleep(3)
by_cmd.send_position_axis_z(20, 0)
var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
pass
def nexec(self):
by_cmd.send_position_axis_z(20, 0)
var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
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)
time.sleep(2)
# 扫黑除暴
class kick_ass():
@@ -1064,9 +1092,8 @@ class kick_ass():
time.sleep(4)
by_cmd.send_position_axis_x(1, 160)
time.sleep(3)
var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
pass
def nexec(self):
var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
pass
def after(self):
var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])