66 lines
1.9 KiB
Python
66 lines
1.9 KiB
Python
|
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")
|