fix: 删除上次提交时调试注释

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

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')

majtask.py

@@ -1,21 +1,9 @@
 
-from simple_pid import PID
 import zmq
 import time
 from loguru import logger
 import utils
-
+from utils import PidWrap
-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)
     
 class main_task():
     def __init__(self,by_cmd):

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__':