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

19
cfg_main.toml
View File

@@ -3,6 +3,7 @@ logger_filename = "log/file_{time}.log"
logger_format = "{time} {level} {message}" logger_format = "{time} {level} {message}"
[task] [task]
Subtask_enable = true # 子任务总使能 调试用！
GetBlock_enable = true # 人员施救使能 GetBlock_enable = true # 人员施救使能
PutBlock_enable = true # 紧急转移使能 PutBlock_enable = true # 紧急转移使能
GetBBall_enable = true # 整装上阵使能 GetBBall_enable = true # 整装上阵使能
@@ -15,16 +16,16 @@ KickAss_enable = true # 扫黑除暴使能
[find_counts] [find_counts]
GetBlock_counts = 10 # 人员施救计数 GetBlock_counts = 10 # 人员施救计数
PutBlock_counts = 5 # 紧急转移计数 PutBlock_counts = 5 # 紧急转移计数
GetBBall_counts = 1 # 整装上阵计数 GetBBall_counts = 1 # 整装上阵计数
UpTower_counts = 3 # 通信抢修计数 UpTower_counts = 3 # 通信抢修计数
GetRBall_counts = 10 # 高空排险计数 GetRBall_counts = 10 # 高空排险计数
PutBBall_counts = 5 # 派发物资计数 PutBBall_counts = 5 # 派发物资计数
PutHanoi1_counts = 10 # 物资盘点计数 PutHanoi1_counts = 10 # 物资盘点计数
PutHanoi2_counts = 2 # 物资盘点计数 PutHanoi2_counts = 2 # 物资盘点计数
PutHanoi3_counts = 2 # 物资盘点计数 PutHanoi3_counts = 2 # 物资盘点计数
MoveArea1_counts = 6 # 应急避险计数 MoveArea1_counts = 6 # 应急避险计数
MoveArea2_counts = 1700 # 应急避险第二阶段计数 MoveArea2_counts = 1700 # 应急避险第二阶段计数
KickAss_counts = 10 # 扫黑除暴计数 KickAss_counts = 10 # 扫黑除暴计数

2
cfg_subtask.toml
View File

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

29
main.py
View File

@@ -20,20 +20,21 @@ 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_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) if cfg_main['task']['Subtask_enable'] is True:
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.get_block1, 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'])) 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.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable']))
# TODO 添加一个空任务用于提前降 z 轴 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.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.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable']))
task_queue.put(sb.task(sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], True)) # 无论是否进行任务，检测标识并转向都是必须进行的 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_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable'])) task_queue.put(sb.task(sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], True)) # 无论是否进行任务，检测标识并转向都是必须进行的
task_queue.put(sb.task(sb.put_hanoi3, 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.put_hanoi3, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_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_area1, cfg_main['find_counts']['MoveArea1_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_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) task_queuem_t = sb.task_queuem(task_queue)

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