diff --git a/cfg_main.toml b/cfg_main.toml index 2cdaa2d..2c7fa4d 100644 --- a/cfg_main.toml +++ b/cfg_main.toml @@ -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 # 整装上阵使能 @@ -15,16 +16,16 @@ KickAss_enable = true # 扫黑除暴使能 [find_counts] GetBlock_counts = 10 # 人员施救计数 -PutBlock_counts = 5 # 紧急转移计数 -GetBBall_counts = 1 # 整装上阵计数 -UpTower_counts = 3 # 通信抢修计数 +PutBlock_counts = 5 # 紧急转移计数 +GetBBall_counts = 1 # 整装上阵计数 +UpTower_counts = 3 # 通信抢修计数 GetRBall_counts = 10 # 高空排险计数 -PutBBall_counts = 5 # 派发物资计数 -PutHanoi1_counts = 10 # 物资盘点计数 -PutHanoi2_counts = 2 # 物资盘点计数 -PutHanoi3_counts = 2 # 物资盘点计数 -MoveArea1_counts = 6 # 应急避险计数 +PutBBall_counts = 5 # 派发物资计数 +PutHanoi1_counts = 10 # 物资盘点计数 +PutHanoi2_counts = 2 # 物资盘点计数 +PutHanoi3_counts = 2 # 物资盘点计数 +MoveArea1_counts = 6 # 应急避险计数 MoveArea2_counts = 1700 # 应急避险第二阶段计数 -KickAss_counts = 10 # 扫黑除暴计数 +KickAss_counts = 10 # 扫黑除暴计数 diff --git a/cfg_subtask.toml b/cfg_subtask.toml index d5d7a0b..3d43aca 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -33,7 +33,7 @@ pid_kd = 0 ################################################ [get_rball] # pid 参数值 -pid_kp = 0.6 +pid_kp = 0.4 pid_ki = 0 pid_kd = 0 diff --git a/main.py b/main.py index b196be4..05e5b15 100644 --- a/main.py +++ b/main.py @@ -20,20 +20,21 @@ logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logge # 向任务队列添加任务 task_queue = queue.Queue() -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'])) -task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) -# TODO 添加一个空任务用于提前降 z 轴 -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_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_hanoi3, 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_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'])) +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'])) + task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) + # TODO 添加一个空任务用于提前降 z 轴 + 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_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_hanoi3, 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_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) diff --git a/subtask.py b/subtask.py index 8c5a324..9749c4b 100644 --- a/subtask.py +++ b/subtask.py @@ -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): - 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): - # 检查该任执行标志 - while True: - ret = self.task_t.find() - self.counts += ret - if self.counts >= self.find_counts: - break - + 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") - self.task_t.exec() - logger.debug(f"[Task ]# Task completed") + if hasattr(self.task_t, 'exec') and callable(getattr(self.task_t, 'exec', None)): + logger.info(f"[Task ]# Executing task") + self.task_t.exec() + else: + logger.warning("[Task ]# 该任务没有 exec 方法") else: 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): 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): - + 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"]) \ No newline at end of file