feat: 增加任务中巡线指定距离指令

2024-06-14 15:11:17 +08:00
parent 2a3993ea62
commit 24a816d6f8
4 changed files with 171 additions and 66 deletions
--- a/action.py
+++ b/action.py
@@ -0,0 +1,99 @@
+import threading
+import time
+import zmq
+import numpy as np
+from loguru import logger
+from simple_pid import PID
+from utils import PidWrap
+
+bycmd = None
+move = None
+
+def import_obj(_bycmd):
+    global bycmd
+    global move
+
+    bycmd = _bycmd
+    move = move_cls()
+
+class lane_cls:
+    def __init__(self, _speed, _time, _pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0}) -> None:
+        self.context = zmq.Context()
+        self.socket = self.context.socket(zmq.REQ)
+        self.socket.connect("tcp://localhost:6666")
+        self.speed = _speed
+        self.time = _time
+        # 每 20ms 控制一次，故次数乘以 50
+        self.count = self.time * 50
+        logger.info(f"count = {self.count}")
+        self.busy = False
+        self. pid = PidWrap(_pid_argv["kp"], _pid_argv["ki"], _pid_argv["kd"], output_limits=50)
+        pass
+    def loop(self):
+        while self.count >= 0:
+            self.count -= 1
+
+            self.socket.send_string("")
+            resp = self.socket.recv_pyobj()
+            # 检查该值是否有效
+            ck_val = resp.get('data')
+            if isinstance(ck_val, np.ndarray):  
+                x = ck_val[0]
+                y = ck_val[1]
+            else:
+                continue
+            err = x - 160
+
+            pid_out = self.pid.get(err)
+            bycmd.send_speed_omega(pid_out)
+
+            # logger.debug(f"[任务中巡线模式]#  error:{err} out:{pid_out}")
+            time.sleep(0.012) # 时间补偿，保证控制周期 (虽然还是挺不准的)
+
+        for _ in range(3):
+            bycmd.send_speed_x(0)
+            bycmd.send_speed_omega(0)
+        self.busy = False
+        logger.info("[任务中巡线模式] #结束")
+        return
+
+    def start(self):
+        logger.info(f"[任务中巡线模式] #开启，速度：{self.speed} 时间：{self.time}")
+        bycmd.send_speed_x(self.speed)
+        self.busy = True
+        thread = threading.Thread(target=self.loop)
+        thread.start()
+        pass
+    def inquire(self):
+        # logger.debug(f"[任务中巡线模式]# 查询 当前 busy 状态 {self.busy}")
+        return self.busy
+
+class move_cls:
+    # def x(self, _speed):
+    #     bycmd.send_speed_x(_speed)
+    #     pass
+    # def y(self, _speed):
+    #     bycmd.send_speed_y(_speed)
+    #     pass
+    # def z(self, _speed):
+    #     bycmd.send_speed_omega(_speed)
+    #     pass
+    def lane(self, _speed, _time):
+        lane_obj = lane_cls(_speed, _time)
+        lane_obj.start()
+        # 此处为阻塞实现，非阻塞调用可直接从 lane_cls 类构造对象，然后使用查询方法
+        # time.sleep(0.5) # 等待子线程启动
+        while lane_obj.inquire() is True:
+            pass
+
+# TODO 新运动指令类，指令的发送和完成查询功能，发送时开启新线程
+
+#  TODO 轴控制指令类，增加指令后加入动作队列，非阻塞
+
+class axis_cls():
+    def axis_z(self, _distance):
+        pass
+    def axis_z2(self, _position):
+        pass
+    def exec(self):
+        pass
--- a/main.py
+++ b/main.py
@@ -6,9 +6,11 @@ import subtask as sb
 import majtask as mj
 from by_cmd_py import by_cmd_py
 import time
+import action as act

 cmd_py_obj = by_cmd_py()
 sb.import_obj(cmd_py_obj)
+act.import_obj(cmd_py_obj)

 # 读取配置
 cfg_main = toml.load('cfg_main.toml')
@@ -16,62 +18,64 @@ cfg_main = toml.load('cfg_main.toml')
 # 配置日志输出
 logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")

-# 向任务队列添加任务
-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'])) 
+act.move.lane(15, 5)

-# 将任务队列传入调度模块中
-task_queuem_t = sb.task_queuem(task_queue)
+# # 向任务队列添加任务
+# 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'])) 

-# 创建任务队列的工作线程
-def worker_thread():
-    while task_queuem_t.exec() is True:
-        pass
+# # 将任务队列传入调度模块中
+# task_queuem_t = sb.task_queuem(task_queue)

-# 启动工作线程
-worker = threading.Thread(target=worker_thread, daemon=True)  
-worker.start()  
-# if (cmd_py_obj.send_angle_camera(180) == -1):
-#     cmd_py_obj.send_angle_camera(180)
-cmd_py_obj.send_position_axis_x(1, 140)
-time.sleep(0.5)
-cmd_py_obj.send_angle_storage(20)
-time.sleep(0.5)
-cmd_py_obj.send_angle_scoop(25)
-time.sleep(2)
+# # 创建任务队列的工作线程
+# def worker_thread():
+#     while task_queuem_t.exec() is True:
+#         pass

-# 创建主任务
-main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
+# # 启动工作线程
+# worker = threading.Thread(target=worker_thread, daemon=True)  
+# worker.start()  
+# # if (cmd_py_obj.send_angle_camera(180) == -1):
+# #     cmd_py_obj.send_angle_camera(180)
+# cmd_py_obj.send_position_axis_x(1, 140)
+# time.sleep(0.5)
+# cmd_py_obj.send_angle_storage(20)
+# time.sleep(0.5)
+# cmd_py_obj.send_angle_scoop(25)
+# time.sleep(2)

-# 主线程仅在子线程搜索 (SEARCHING) 和 空闲 (IDLE) 状态下进行操作
-# while task_queuem_t.busy is True:
-try:
-    while True:
-        if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
-            pass
-        else:
-            main_task_t.run()
-            pass
-except KeyboardInterrupt:
-    logger.info("Interrupt received, stopping...")
-    # 停车 
-    for _ in range(3):
-        cmd_py_obj.send_speed_x(0)
-        time.sleep(0.1)
-        cmd_py_obj.send_speed_omega(0)
-        time.sleep(0.1)
+# # 创建主任务
+# main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象

-logger.info("Main thread exit")
+# # 主线程仅在子线程搜索 (SEARCHING) 和 空闲 (IDLE) 状态下进行操作
+# # while task_queuem_t.busy is True:
+# try:
+#     while True:
+#         if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
+#             pass
+#         else:
+#             main_task_t.run()
+#             pass
+# except KeyboardInterrupt:
+#     logger.info("Interrupt received, stopping...")
+#     # 停车 
+#     for _ in range(3):
+#         cmd_py_obj.send_speed_x(0)
+#         time.sleep(0.1)
+#         cmd_py_obj.send_speed_omega(0)
+#         time.sleep(0.1)
+
+# logger.info("Main thread exit")
--- a/majtask.py
+++ b/majtask.py
@@ -1,21 +1,9 @@

-from simple_pid import PID
 import zmq
 import time
 from loguru import logger
 import utils
-
-class PidWrap:
-    def __init__(self, kp, ki, kd, setpoint=0, output_limits=1):
-        self.pid_t = PID(kp, ki, kd, setpoint, output_limits=(0-output_limits, output_limits))
-    def set_target(self, target):
-        self.pid_t.setpoint = target
-    def set(self, kp, ki, kd):
-        self.pid_t.Kp = kp
-        self.pid_t.Ki = ki
-        self.pid_t.Kd = kd
-    def get(self, val_in):
-        return self.pid_t(val_in)
+from utils import PidWrap
    
 class main_task():
    def __init__(self,by_cmd):
--- a/utils.py
+++ b/utils.py
@@ -2,6 +2,8 @@
 from enum import Enum
 import numpy as np
 import erniebot
+from simple_pid import PID
+
 # 巡线误差
 lane_error = 0
 # 进入任务时可以通过修改 task_speed 控制巡线速度
@@ -302,6 +304,18 @@ class CountRecord:
        else:
            return False

+class PidWrap:
+    def __init__(self, kp, ki, kd, setpoint=0, output_limits=1):
+        self.pid_t = PID(kp, ki, kd, setpoint, output_limits=(0-output_limits, output_limits))
+    def set_target(self, target):
+        self.pid_t.setpoint = target
+    def set(self, kp, ki, kd):
+        self.pid_t.Kp = kp
+        self.pid_t.Ki = ki
+        self.pid_t.Kd = kd
+    def get(self, val_in):
+        return self.pid_t(val_in)
+


 if __name__ == '__main__':