pref: 修改子任务为类

mtask.py

@@ -0,0 +1,66 @@
+from simple_pid import PID
+
+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,socket):
+        self.lane_socket = socket
+
+        # 赛道回归相关
+        self.x = 0
+        self.y = 0
+        self.error_counts = 0
+        self.lane_error = 0
+
+        # 车控制对象初始化
+        # self.by_cmd = by_cmd_py()
+
+        # 转向 pid
+        self.pid1 = PidWrap(0.7, 0, 0,output_limits=40)
+        self.pid1.set_target(0)
+
+    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
+            
+        else:
+            pass
+    def run(self):
+        try:
+            data = self.queen.get_nowait()
+            self.parse_data(data)
+        except:
+            pass
+        # 运行巡线任务
+        self.lane_task()
+            
+    def lane_task(self):
+        # TODO 巡航参数从配置文件中读取
+        if self.error_counts > 2:
+            self.x = self.x / 3
+            self.y = self.y / 3
+            self.lane_error = self.x - 160
+            self.error_counts = 0
+            self.x = 0
+            self.y = 0
+            if self.lane_error > 30:
+                self.pid1.set(0.7, 0, 0)
+            else:
+                self.pid1.set(0.5, 0, 0)
+            # 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")