pref: 注册时直接传入任务类

feat: 分任务设置检测计数值

majtask.py

@@ -1,5 +1,7 @@
 from simple_pid import PID
-# import queue
+import zmq
+import time
+from loguru import logger
 
 class PidWrap:
     def __init__(self, kp, ki, kd, setpoint=0, output_limits=1):
@@ -14,8 +16,10 @@ class PidWrap:
         return self.pid_t(val_in)
     
 class main_task():
-    def __init__(self,socket):
-        self.lane_socket = socket
+    def __init__(self,by_cmd):
+        self.context = zmq.Context()
+        self.socket = self.context.socket(zmq.REQ)
+        self.socket.connect("tcp://localhost:6666")
 
         # 赛道回归相关
         self.x = 0
@@ -24,7 +28,7 @@ class main_task():
         self.lane_error = 0
 
         # 车控制对象初始化
-        # self.by_cmd = by_cmd_py()
+        self.by_cmd = by_cmd
 
         # 转向 pid
         self.pid1 = PidWrap(0.7, 0, 0,output_limits=40)
@@ -32,10 +36,9 @@ class main_task():
 
     def parse_data(self,data):
         if data.get('code') == 0:
-            if data.get('type') == 'infer':
-                self.x += data.get('data')[0][0]
-                self.y += data.get('data')[0][1]
-                self.error_counts += 1
+            self.x += data.get('data')[0]
+            self.y += data.get('data')[1]
+            self.error_counts += 1
             
         else:
             pass
@@ -52,6 +55,7 @@ class main_task():
             
     def lane_task(self):
         # TODO 巡航参数从配置文件中读取
+        time.sleep(0.002)
         if self.error_counts > 2:
             self.x = self.x / 3
             self.y = self.y / 3
@@ -59,12 +63,27 @@ class main_task():
             self.error_counts = 0
             self.x = 0
             self.y = 0
-            if self.lane_error > 30:
+            error_abs = abs(self.lane_error)
+            if error_abs < 10:
                 self.pid1.set(0.7, 0, 0)
+                self.by_cmd.send_speed_x(12)
+            elif error_abs > 45:
+                self.by_cmd.send_speed_x(6)
+                self.pid1.set(1.8, 0, 0)
+            elif error_abs > 35:
+                self.by_cmd.send_speed_x(8)
+                self.pid1.set(1.5, 0, 0)
+            elif error_abs > 25:
+                self.by_cmd.send_speed_x(10)
+                self.pid1.set(1, 0, 0)
             else:
-                self.pid1.set(0.5, 0, 0)
+                self.pid1.set(0.8, 0, 0)
+                self.by_cmd.send_speed_x(11)
             # TODO 待引入控制接口
-            # self.by_cmd.send_speed_x(7)
-            pid_out = self.pid1.get(self.lane_error)
-            # self.by_cmd.send_speed_omega(pid_out)
-        # self.lane_socket.send_string("infer")
+            
+            pid_out = self.pid1.get(self.lane_error*0.65)
+            self.by_cmd.send_speed_omega(pid_out)
+        self.socket.send_string("")
+        resp = self.socket.recv_pyobj()
+        # logger.info(resp)
+        self.parse_data(resp)