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

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