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

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