LSX183
/
WRC
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: LSX183:master
Branches Tags
LSX183:master
LSX183:master_pure
..
compare: LSX183:master_pure
Branches Tags
LSX183:master
LSX183:master_pure

2 Commits
master ... master_pur

Author SHA1 Message Date
liu a9acb32214 V0.1 倒车程序 1 year ago
liu 90323686b6 V0.3 纯跟踪更稳定 1 year ago
6 changed files with 745 additions and 1309 deletions
Show Stats

134
gear_change.py
Unescape Escape View File

@ -1,134 +0,0 @@
import sqlite3
import time
def init():
gear_cmd = 0
auto_acc=0
brk_open=0
brk_change=0
stop=0
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s',brk_open='%s',brk_change='%s' ,stop='%s'WHERE id = 1"%(
gear_cmd,auto_acc,brk_open,brk_change,stop)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_p():
print('P')
gear_cmd = 1
auto_acc=0
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_n():
print('N')
gear_cmd = 3 # n
auto_acc=0
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_r():
gear_cmd = 2 # r
print('r')
auto_acc = input("请输入油门深度：")
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_d():
gear_cmd = 4 # r
print('d')
auto_acc = input("请输入油门深度：")
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def shache():
brk_open = 30
brk_change=1
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set brk_open='%s',brk_change='%s' WHERE id = 1"%(brk_open,brk_change)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def shache2():
brk_change=0
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set brk_change='%s' WHERE id = 1"%brk_change
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def stop():
stop=1
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set stop='%s' WHERE id = 1"%stop
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def main():
while True:
gear_change = int(input('请输入档位：\n'
'1:P档\n'
'2:R档\n'
'3:N档\n'
'4:D档\n'
'5:刹车\n'
'6:紧急停止\n'))
if gear_change==1:
gear_p()
elif gear_change==3:
gear_n()
elif gear_change==2:
gear_r()
elif gear_change==4:
gear_d()
elif gear_change==5:
shache()
time.sleep(0.1)
shache2()
elif gear_change==6:
stop()
else:
print('请重新输入')
if __name__=='__main__':
init()
main()

235
geartest2.py
Unescape Escape View File

@ -1,235 +0,0 @@
import os
import datetime
import threading
import time
import can # 需要安装python-can库 pip install python-can
from can import bus
import geardb
# def option():
# global auto_acc, gear_cmd, brk_open, brk_change, stop
# auto_acc = 0 # 加速踏板行程0~100%
# brk_open = 100 # 刹车深度0-100%
# gear_cmd = 4 # 需求档位 1:P 2:R 3:N 4:D
# brk_change = 0 # 默认为0 变更为1
# stop = 0 # 紧急停止 默认为0
# can1 = can.interface.Bus(channel = 'can0', bustype = 'socketcan') #python-can 4.0需要用这个
def update():
global gear_cmd,auto_acc,brk_open,brk_change,stop
while True:
date = geardb.table_quer()
gear_cmd = int(date[0][1]) # 需求档位 1:P 2:R 3:N 4:D
auto_acc = int(date[0][2]) # 加速踏板行程0~100%
brk_open = int(date[0][3]) # 刹车深度0-100%
brk_change = int(date[0][4]) # 默认为0 变更为1
stop = int(date[0][5]) # 紧急停止 默认为0
time.sleep(0.05)
# 获取当前档位
def can4b8():
global gear_act
bus.set_filters = [{"can_id": 0x04b8, "can_mask": 0xFFFF, "extended": False}]
# 连接can1
can1 = can.interface.Bus(channel='can1', bustype='socketcan', can_filters=bus.set_filters) # python-can 4.0需要用这个
# 一直循环接收数据
while True:
msg = can1.recv(3.0) # recv()中定义超时接收时间
# print(msg)
can4b81 = str(msg).replace(' ', '')
can4b8id = can4b81.find('ID')
can4b8dl = can4b81.find('DL')
can_id = can4b81[can4b8id + 3:can4b8id + 7] # id
# print(can_id)
# print(can4b81)
# print(can4b8id)
gear_act0 = can4b81[can4b8dl + 10:can4b8dl + 11] # gear
a = int(gear_act0)
if can_id == '04b8' and a > 1:
gear_act1 = bin(int(gear_act0))
gear_act2 = gear_act1[:-1]
gear_act = int(gear_act2, 2)
# if gear_act == 1:
# print('实际档位：为P档')
# elif gear_act==2:
# print('实际档位：为R档')
# elif gear_act==3:
# print('实际档位：为N档')
# elif gear_act==4:
# print('实际档位：为D档')
# else:
# print('档位识别失败')
else:
print('未获取到实际挡位')
# 刹车转换
def brk_open_conversion(brk_open10):
global brk_open_low, brk_open_high
if 100 >= brk_open10 >= 0:
brk_open = brk_open10 * 100
brk_open16 = hex(brk_open)
# print(brk_open16)
brk_open_all = brk_open16[2:].upper()
if len(str(brk_open_all)) == 1:
brk_open_low = "0%s" % brk_open_all
brk_open_high = '00'
elif len(str(brk_open_all)) == 2:
brk_open_low = brk_open_all
brk_open_high = '00'
elif len(str(brk_open_all)) == 3:
brk_open_low = brk_open_all[1: 3]
brk_open_high = "0%s" % brk_open_all[0: 1]
elif len(str(brk_open_all)) == 4:
brk_open_low = brk_open_all[2: 4]
brk_open_high = brk_open_all[0: 2]
# print(brk_open_low, brk_open_high)
else:
print('刹车深度输入范围0~100')
# 油门转换
def acc_conversion(auto_acc10):
global auto_acc_all
if 100 >= auto_acc10 >= 0:
auto_acc16 = hex(auto_acc10)
# print(auto_acc16)
auto_acc_all16 = auto_acc16[2:].upper()
# print(auto_acc_all16)
if len(str(auto_acc_all16)) == 1:
auto_acc_all = "0%s" % auto_acc_all16
else:
auto_acc_all = auto_acc_all16
# print(auto_acc_all)
else:
print('加速踏板深度输入范围0~100')
# 设置刹车
def brk_open_option():
global brk_change
while True:
brk_open_conversion(brk_open)
if brk_change == 1:
print('cansend can2 121#%s%s010000000000' % (brk_open_low, brk_open_high))
os.system('cansend can2 121#%s%s010000000000' % (brk_open_low, brk_open_high))
brk_change = 0
time.sleep(0.01)
# 设置P档
def gear_P():
os.system('cansend can0 161#7200000500000000') # 挂p档切自动驾驶模式
# print('cansend can0 161#7200000500000000') # 挂p档切自动驾驶模式
# 设置N档
def gear_N():
os.system('cansend can0 161#7200000000000000') # 挂N档切自动驾驶模式
# print('cansend can0 161#7200000000000000') # 挂N
# 保持D档
def gear_keep_D():
acc_conversion(int(auto_acc))
os.system('cansend can0 161#72%s000400000000' % auto_acc_all) # 挂D档切自动驾驶模式
# print('cansend can0 161#72%s000400000000' % auto_acc_all) # 挂D档切自动驾驶模式
# 保持R档
def gear_keep_R():
acc_conversion(int(auto_acc))
os.system('cansend can0 161#72%s000700000000' % auto_acc_all) # 挂R档切自动驾驶模式
# print('cansend can0 161#72%s000700000000' % auto_acc_all) # 挂R档切自动驾驶模式
# 切换D档
def gear_D():
os.system('cansend can2 121#1027010000000000') # 踩刹车
acc_conversion(int(auto_acc))
os.system('cansend can0 161#72%s000400000000' % auto_acc_all) # 挂D档切自动驾驶模式
os.system('cansend can2 121#0000010000000000') # 松刹车
# print('cansend can0 161#72%s000400000000' % auto_acc_all) # 挂D档切自动驾驶模式
def gear_R():
os.system('cansend can2 121#1027010000000000') # 踩刹车
acc_conversion(int(auto_acc))
os.system('cansend can0 161#72%s000700000000' % auto_acc_all) # 挂R档切自动驾驶模式
os.system('cansend can2 121#0000010000000000') # 松刹车
# print('cansend can0 161#72%s000700000000' % auto_acc_all) # 挂R档切自动驾驶模式
def main():
while True:
if stop == 0:
print('当前档位：',gear_act)
if gear_cmd == 1:
if gear_act == 1:
print('当前已是P档')
else:
print('切换P档')
gear_P()
elif gear_cmd == 3:
if gear_act == 3:
print('当前已是N档')
else:
print('切换N档')
gear_N()
elif gear_cmd == 4:
if gear_act == 4:
print('当前已是D档')
gear_keep_D()
else:
print('切换D档')
gear_D()
# os.system('cansend can2 121#0000010000000000') #测试用
elif gear_cmd == 2:
if gear_act == 2:
print('当前已是R档')
gear_keep_R()
else:
print('切换R档')
gear_R()
else:
print('未设定档位')
else:
print('紧急停止')
os.system('cansend can2 121#1027010000000000')
break
time.sleep(0.01)
# 刹车初始值 100%
brk_open_low = '10'
brk_open_high = '27'
# 初始加速踏板行程0
auto_acc16 = '00'
gear_act = 0 # 实际挡位
gear_cmd = 4 # 需求档位 1:P 2:R 3:N 4:D
auto_acc = 0 # 加速踏板行程0~100%
brk_open = 100 # 刹车深度0-100%
brk_change = 0 # 默认为0 变更为1
stop = 0 # 紧急停止 默认为0
# if __name__ == '__main__':
# # t1 = threading.Thread(target=option)
# t2 = threading.Thread(target=brk_open_option)
# t3 = threading.Thread(target=can4b8)
# t4 = threading.Thread(target=main)
# t5 = threading.Thread(target=update)
# # option()
# # t1.start()
# t2.start()
# t3.start()
# t4.start()
# t5.start()

376
pure_back_right.py
Unescape Escape View File

@ -0,0 +1,376 @@
import time
from loguru import logger
import numpy as np
import math
# import matplotlib.pyplot as plt
import tcp4
import sqlite3
import threading
import test5
import sped
k = 0 # 前视距离系数
Lfc = 5 # 前视距离
Kp = 1.0 # 速度P控制器系数
dt = 0.1 # 时间间隔，单位：s
L = 2.7 # 车辆轴距，单位：m
fangxp = 0
class VehicleState:
# state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0)
def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a, delta):
state.x = tcp4.x_car + sped.speed * math.cos(tcp4.yaw) * dt
# print("state",state.x)
state.y = tcp4.y_car + sped.speed * math.sin(tcp4.yaw) * dt
state.yaw = tcp4.yaw + sped.speed / L * math.tan(delta) * dt
state.v = sped.speed + a * dt
return state
def PControl(target, current):
a = Kp * (target - current)
return a
def pure_pursuit_control(state, cx, cy, pind):
global fangxp
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
print('tx', tx)
ty = cy[ind]
print('ty', ty)
else:
tx = cx[-1]
# print('tx1',tx)
ty = cy[-1]
# print('ty1',ty)
ind = len(cx) - 1
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
# print('alpha',alpha)
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
if delta > np.pi / 6.0:
delta = np.pi / 6.0
# print('delta1', delta)
elif delta < - np.pi / 6.0:
delta = - np.pi / 6.0
# print('delta2', delta)
angle = delta * 57.3
fangxp = int(angle * 18)
print('fxp', fangxp)
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
time.sleep(0.04)
# print(delta)
return delta, ind
def calc_target_index(state, cx, cy):
# 搜索最临近的路点
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
# print('ind',ind)
L = 0.0
Lf = k * state.v + Lfc
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
# print('ind1', ind)
return ind
def main():
# 设置目标路点
cx = [84.61, 84.639, 84.667, 84.7, 84.722, 84.743, 84.775, 84.806, 84.837, 84.869, 84.9, 84.928, 84.949, 84.972,
85.004, 85.028, 85.06, 85.096, 85.123, 85.149, 85.177, 85.208, 85.24, 85.269, 85.298, 85.331, 85.361, 85.391,
85.423,
85.457, 85.488, 85.526, 85.547, 85.572, 85.602, 85.622, 85.648, 85.673, 85.694, 85.721, 85.741, 85.762,
85.786, 85.81, 85.832, 85.856, 85.88, 85.899, 85.923, 85.948, 85.973, 85.996, 86.017, 86.042, 86.068, 86.089,
86.113,
86.131,
86.154, 86.175, 86.195, 86.217, 86.245, 86.262, 86.288, 86.307, 86.33, 86.35, 86.376, 86.398, 86.423,
86.448, 86.474, 86.499, 86.52, 86.548, 86.575, 86.602, 86.624, 86.65, 86.676, 86.7, 86.72, 86.743, 86.77,
86.793, 86.821, 86.836, 86.861, 86.887, 86.916, 86.943, 86.964, 86.988, 87.01, 87.039, 87.066, 87.092, 87.12,
87.145,
87.169, 87.192, 87.221, 87.243, 87.271, 87.295, 87.324, 87.348, 87.369, 87.391, 87.417, 87.445, 87.475,
87.501, 87.525, 87.546, 87.575, 87.6, 87.629, 87.656, 87.685, 87.712, 87.736, 87.77, 87.794, 87.823, 87.852,
87.885,
87.916, 87.939, 87.963, 87.994, 88.019, 88.053, 88.084, 88.114, 88.138, 88.17, 88.196, 88.228, 88.257, 88.288,
88.318, 88.343, 88.373, 88.4, 88.427, 88.461, 88.489, 88.518, 88.548, 88.58, 88.606, 88.633, 88.66, 88.693,
88.712, 88.738, 88.765, 88.791, 88.826, 88.861, 88.888, 88.914, 88.943, 88.969, 88.996, 89.022, 89.052,
89.082,
89.113, 89.135, 89.158, 89.184, 89.218, 89.248, 89.272, 89.297, 89.334, 89.362, 89.39, 89.412, 89.445, 89.475,
89.504,
89.529, 89.561, 89.589, 89.617, 89.65, 89.673, 89.713, 89.736, 89.764, 89.792, 89.825, 89.853, 89.88, 89.913,
89.939, 89.968, 89.997, 90.026, 90.057, 90.086, 90.119, 90.144, 90.165, 90.194, 90.226, 90.251, 90.279,
90.306, 90.336, 90.365, 90.393, 90.424, 90.447, 90.477, 90.508, 90.533, 90.568, 90.601, 90.627, 90.654,
90.685,
90.713, 90.745, 90.776, 90.806, 90.835, 90.862, 90.894, 90.922, 90.949, 90.982, 91.009, 91.043, 91.069,
91.102,
91.138, 91.168, 91.199, 91.228, 91.26, 91.288, 91.316, 91.345, 91.372, 91.407, 91.436, 91.464, 91.494, 91.522,
91.552,
91.578, 91.603, 91.629, 91.66, 91.69, 91.713, 91.742, 91.774, 91.803, 91.823, 91.851, 91.877, 91.908, 91.935,
91.963, 91.993, 92.019, 92.045, 92.071, 92.096, 92.127, 92.154, 92.184, 92.207, 92.233, 92.264, 92.297,
92.324, 92.347, 92.374, 92.403, 92.43, 92.459, 92.487, 92.513, 92.544, 92.568, 92.596, 92.623, 92.652, 92.681,
92.706,
92.732, 92.762, 92.79, 92.826, 92.856, 92.881, 92.91, 92.936, 92.967, 92.991, 93.023, 93.047, 93.075, 93.105,
93.127, 93.155, 93.184, 93.214, 93.24, 93.263, 93.29, 93.32, 93.348, 93.376, 93.407, 93.433, 93.465, 93.498,
93.524, 93.559, 93.588, 93.621, 93.647, 93.675, 93.703, 93.735, 93.763, 93.797, 93.83, 93.858, 93.888, 93.914,
93.944, 93.975, 94.001, 94.026, 94.052, 94.083, 94.11, 94.141, 94.164, 94.188, 94.213, 94.24, 94.27, 94.296,
94.328, 94.355, 94.389, 94.418, 94.451, 94.48, 94.509, 94.535, 94.577, 94.607, 94.639, 94.672, 94.702, 94.724,
94.756, 94.786, 94.816, 94.847, 94.879, 94.907, 94.941, 94.97, 94.999, 95.028, 95.055, 95.089, 95.12, 95.149,
95.177, 95.215, 95.244, 95.275, 95.29, 95.32, 95.351, 95.379, 95.406, 95.435, 95.466, 95.493, 95.522, 95.546,
95.576, 95.612, 95.642, 95.674, 95.706, 95.732, 95.782, 95.812, 95.844, 95.872, 95.9, 95.925, 95.95, 95.985,
96.014, 96.048, 96.078, 96.109, 96.139, 96.17, 96.203, 96.234, 96.266, 96.299, 96.33, 96.364, 96.395, 96.424,
96.456, 96.487, 96.514, 96.546, 96.576, 96.609, 96.638, 96.667, 96.701, 96.735, 96.765, 96.795, 96.825,
96.855, 96.888, 96.918, 96.953, 96.982, 97.013, 97.045, 97.095, 97.123, 97.157, 97.179, 97.208, 97.243,
97.277,
97.308, 97.342, 97.376, 97.407, 97.435, 97.47, 97.501, 97.534, 97.565, 97.595, 97.624, 97.649, 97.684, 97.716,
97.749,
97.776, 97.811, 97.84, 97.869, 97.901, 97.931, 97.96, 97.984, 98.016, 98.042, 98.07, 98.1, 98.128, 98.155,
98.188, 98.218, 98.244, 98.273, 98.304, 98.339, 98.366, 98.397, 98.42, 98.465, 98.495, 98.523, 98.549, 98.578,
98.61,
98.637, 98.667, 98.7, 98.729, 98.757, 98.789, 98.819, 98.843, 98.872, 98.892, 98.921, 98.955, 98.984, 99.01,
99.04, 99.06, 99.09, 99.121, 99.152, 99.178, 99.208, 99.233, 99.26, 99.29, 99.32, 99.349, 99.376, 99.4,
99.428,
99.456, 99.486, 99.514, 99.544, 99.571, 99.598, 99.627, 99.654, 99.682, 99.714, 99.741, 99.769, 99.799,
99.831,
99.859, 99.888, 99.932, 99.957, 99.987, 100.012, 100.043, 100.076, 100.108, 100.135, 100.167, 100.191,
100.221,
100.252, 100.281, 100.311, 100.342, 100.371, 100.394, 100.427, 100.455, 100.49, 100.522, 100.553, 100.58,
100.614,
100.641, 100.672, 100.702, 100.728, 100.761, 100.792, 100.824, 100.849, 100.876, 100.909, 100.942, 100.967,
100.999,
101.031, 101.059, 101.09, 101.121, 101.15, 101.175, 101.204, 101.235, 101.264, 101.288, 101.32, 101.348,
101.38, 101.404, 101.433, 101.468, 101.495, 101.521, 101.551, 101.575, 101.604, 101.632, 101.66, 101.683,
101.716,
101.744, 101.776, 101.808, 101.83, 101.854, 101.889, 101.918, 101.945, 101.967, 101.99, 102.013, 102.033,
102.057,
102.082, 102.102, 102.116, 102.134, 102.149, 102.164, 102.176, 102.184, 102.187, 102.185, 102.183, 102.176,
102.165,
102.141, 102.112, 102.086, 102.058, 102.031, 101.989, 101.955, 101.912, 101.861, 101.805, 101.748, 101.68,
101.606, 101.522, 101.44, 101.353, 101.272, 101.185, 101.09, 100.999, 100.9, 100.804, 100.706, 100.611,
100.505,
100.406, 100.299, 100.195, 100.085, 99.979, 99.867, 99.739, 99.62, 99.493, 99.363, 99.231, 99.102, 98.965,
98.83,
98.623, 98.48, 98.339, 98.198, 98.06, 97.927, 97.794, 97.659, 97.534, 97.402, 97.268, 97.143, 97.023, 96.896,
96.773,
96.645, 96.518, 96.393, 96.26, 96.127, 95.988, 95.845, 95.706, 95.558, 95.409, 95.258, 95.102, 94.945, 94.788,
94.633, 94.475, 94.315, 94.16, 94.002, 93.903, 93.75, 93.589, 93.434, 93.274, 93.118, 92.963, 92.807, 92.648,
92.487, 92.333, 92.18, 92.022, 91.864, 91.711, 91.553, 91.407, 91.252, 91.102, 90.954, 90.806, 90.652, 90.511,
90.354, 90.203, 90.049, 89.89, 89.737, 89.581, 89.422, 89.26, 89.108, 88.949, 88.793, 88.636, 88.48, 88.326,
88.168, 88.01, 87.855, 87.7, 87.548, 87.395, 87.243, 87.088, 86.939, 86.785, 86.635, 86.482, 86.334, 86.185,
86.038, 85.886, 85.666, 85.516, 85.367, 85.217, 85.067, 84.913, 84.766, 84.62, 84.473, 84.322, 84.171, 84.022,
83.871, 83.722, 83.576, 83.429, 83.282, 83.132, 82.981, 82.834, 82.687, 82.524, 82.375, 82.225, 82.072,
81.918, 81.772, 81.618, 81.457, 81.309, 81.154, 80.998, 80.843, 80.694, 80.543, 80.385, 80.237, 80.086,
79.937,
79.785, 79.639, 79.493, 79.355, 79.205, 79.063, 78.922, 78.784, 78.647, 78.517, 78.384, 78.25, 78.119, 77.986,
77.856,
77.741, 77.625, 77.511, 77.403, 77.298, 77.199, 77.065, 76.938, 76.826, 76.707]
cy = [66.014, 65.909, 65.805, 65.693, 65.595, 65.496, 65.384, 65.267, 65.144, 65.021, 64.894, 64.796, 64.695,
64.595, 64.484, 64.373, 64.26, 64.141, 64.023, 63.9, 63.776, 63.65, 63.521, 63.395, 63.261, 63.128, 62.997,
62.864, 62.734, 62.595, 62.453, 62.312, 62.214, 62.114, 61.97, 61.87, 61.769, 61.667, 61.565, 61.464, 61.362,
61.257, 61.157, 61.05, 60.944, 60.842, 60.735, 60.63, 60.523, 60.414, 60.306, 60.198, 60.091, 59.98, 59.867,
59.755, 59.645, 59.535, 59.419, 59.308, 59.196, 59.088, 58.971, 58.855, 58.736, 58.616, 58.494, 58.373,
58.253, 58.133, 58.012, 57.892, 57.767, 57.645, 57.525, 57.404, 57.282, 57.163, 57.041, 56.922, 56.801,
56.682, 56.562, 56.442, 56.323, 56.207, 56.081, 55.967, 55.835, 55.715, 55.585, 55.454, 55.327, 55.196, 55.07,
54.942, 54.814, 54.69, 54.572, 54.443, 54.317, 54.191, 54.067, 53.949, 53.83, 53.713, 53.603, 53.483, 53.364,
53.244, 53.126, 53.007, 52.882, 52.76, 52.634, 52.507, 52.386, 52.26, 52.133, 52.002, 51.873, 51.741, 51.607,
51.472, 51.34, 51.205, 51.072, 50.934, 50.794, 50.655, 50.512, 50.376, 50.235, 50.095, 49.951, 49.808, 49.666,
49.522, 49.381, 49.237, 49.094, 48.955, 48.814, 48.678, 48.541, 48.403, 48.254, 48.045, 47.903, 47.762,
47.626, 47.483, 47.337, 47.193, 47.049, 46.903, 46.759, 46.613, 46.465, 46.313, 46.168, 46.027, 45.881, 45.74,
45.599, 45.457, 45.309, 45.166, 45.021, 44.875, 44.729, 44.582, 44.439, 44.292, 44.143, 43.997, 43.851,
43.709, 43.568, 43.426, 43.279, 43.136, 42.994, 42.849, 42.704, 42.561, 42.418, 42.273, 42.127, 41.986,
41.844, 41.7, 41.552, 41.404, 41.257, 41.112, 40.964, 40.818, 40.672, 40.526, 40.381, 40.236, 40.09, 39.941,
39.797, 39.653, 39.512, 39.362, 39.223, 39.08, 38.94, 38.795, 38.653, 38.508, 38.368, 38.226, 38.078, 37.931,
37.788, 37.639, 37.496, 37.351, 37.206, 37.059, 36.908, 36.763, 36.613, 36.463, 36.319, 36.172, 36.024,
35.877, 35.729, 35.58, 35.434, 35.286, 35.139, 34.995, 34.844, 34.695, 34.552, 34.402, 34.256, 34.107, 33.958,
33.813, 33.663, 33.512, 33.363, 33.211, 33.067, 32.922, 32.773, 32.627, 32.484, 32.336, 32.19, 32.047, 31.903,
31.76, 31.61, 31.469, 31.325, 31.183, 31.039, 30.901, 30.762, 30.625, 30.488, 30.349, 30.213, 30.074, 29.935,
29.797, 29.661, 29.522, 29.393, 29.254, 29.102, 28.959, 28.815, 28.671, 28.524, 28.383, 28.241, 28.104,
27.968, 27.823, 27.69, 27.55, 27.409, 27.267, 27.128, 26.985, 26.842, 26.698, 26.554, 26.406, 26.258, 26.107,
25.96, 25.808, 25.664, 25.513, 25.366, 25.215, 25.069, 24.927, 24.791, 24.654, 24.513, 24.374, 24.234, 24.098,
23.954, 23.812, 23.664, 23.521, 23.375, 23.228, 23.083, 22.934, 22.789, 22.645, 22.499, 22.351, 22.205,
22.057, 21.913, 21.767, 21.628, 21.484, 21.337, 21.191, 21.046, 20.9, 20.758, 20.618, 20.476, 20.33, 20.191,
20.049, 19.908, 19.766, 19.632, 19.489, 19.353, 19.217, 19.076, 18.935, 18.797, 18.659, 18.516, 18.374,
18.227, 18.085, 17.935, 17.789, 17.64, 17.495, 17.344, 17.119, 16.969, 16.823, 16.669, 16.521, 16.371, 16.219,
16.072, 15.926, 15.775, 15.629, 15.483, 15.341, 15.195, 15.05, 14.901, 14.757, 14.61, 14.463, 14.315, 14.167,
14.023, 13.871, 13.726, 13.582, 13.436, 13.291, 13.147, 13.007, 12.863, 12.725, 12.586, 12.446, 12.31, 12.175,
12.043, 11.898, 11.76, 11.621, 11.473, 11.259, 11.114, 10.97, 10.823, 10.671, 10.523, 10.375, 10.233, 10.082,
9.935, 9.788, 9.642, 9.494, 9.342, 9.194, 9.046, 8.896, 8.747, 8.598, 8.451, 8.302, 8.158, 8.012, 7.864,
7.718, 7.569, 7.425, 7.28, 7.132, 6.986, 6.841, 6.693, 6.543, 6.396, 6.246, 6.099, 5.95, 5.801, 5.653, 5.5,
5.351, 5.2, 4.974, 4.821, 4.669, 4.522, 4.373, 4.222, 4.068, 3.918, 3.77, 3.623, 3.472, 3.322, 3.173, 3.025,
2.882, 2.733, 2.583, 2.436, 2.289, 2.144, 1.994, 1.845, 1.695, 1.546, 1.397, 1.245, 1.091, 0.939, 0.791,
0.641, 0.493, 0.346, 0.197, 0.051, -0.102, -0.254, -0.404, -0.553, -0.708, -0.859, -1.012, -1.166, -1.321,
-1.474, -1.628, -1.858, -2.012, -2.164, -2.32, -2.473, -2.626, -2.776, -2.927, -3.078, -3.231, -3.384, -3.535,
-3.686, -3.836, -3.987, -4.136, -4.287, -4.43, -4.58, -4.728, -4.872, -5.013, -5.158, -5.302, -5.442, -5.585,
-5.728, -5.873, -6.015, -6.158, -6.305, -6.447, -6.59, -6.733, -6.877, -7.022, -7.165, -7.307, -7.449, -7.59,
-7.732, -7.871, -8.012, -8.151, -8.29, -8.429, -8.576, -8.718, -8.863, -9.006, -9.153, -9.371, -9.514, -9.657,
-9.789, -9.937, -10.083, -10.229, -10.378, -10.523, -10.668, -10.812, -10.958, -11.103, -11.247, -11.395,
-11.54, -11.689, -11.832, -11.978, -12.121, -12.267, -12.412, -12.559, -12.705, -12.85, -13.006, -13.15,
-13.3, -13.447, -13.593, -13.741, -13.889, -14.033, -14.182, -14.326, -14.474, -14.607, -14.754, -14.903,
-15.051, -15.195, -15.346, -15.491, -15.642, -15.794, -15.945, -16.091, -16.249, -16.401, -16.546, -16.696,
-16.843, -16.986, -17.132, -17.272, -17.413, -17.553, -17.694, -17.839, -17.984, -18.128, -18.271, -18.413,
-18.558, -18.705, -18.857, -19.008, -19.149, -19.293, -19.441, -19.586, -19.73, -19.876, -20.023, -20.169,
-20.317, -20.462, -20.61, -20.763, -20.912, -21.067, -21.225, -21.373, -21.528, -21.675, -21.818, -21.953,
-22.076, -22.203, -22.372, -22.481, -22.601, -22.718, -22.832, -22.95, -23.07, -23.192, -23.315, -23.439,
-23.56, -23.68, -23.799, -23.912, -24.022, -24.132, -24.242, -24.34, -24.435, -24.518, -24.605, -24.686,
-24.761, -24.838, -24.911, -24.978, -25.039, -25.1, -25.154, -25.213, -25.253, -25.307, -25.361, -25.408,
-25.451, -25.493, -25.53, -25.573, -25.62, -25.642, -25.665, -25.683, -25.703, -25.718, -25.732, -25.747,
-25.758, -25.759, -25.769, -25.778, -25.789, -25.799, -25.799, -25.8, -25.8, -25.8, -25.8, -25.807, -25.816,
-25.823, -25.836, -25.841, -25.861, -25.884, -25.908, -25.936, -25.961, -25.99, -26.017, -26.048, -26.084,
-26.118, -26.141, -26.176, -26.213, -26.247, -26.274, -26.299, -26.34, -26.373, -26.403, -26.435, -26.467,
-26.5, -26.532, -26.563, -26.599, -26.624, -26.661, -26.683, -26.715, -26.748, -26.78, -26.813, -26.842,
-26.873, -26.908, -26.943, -26.977, -27.008, -27.043, -27.068, -27.095, -27.133, -27.164, -27.193, -27.224,
-27.255, -27.287, -27.319, -27.348, -27.381, -27.407, -27.448, -27.483, -27.518, -27.55, -27.587, -27.615,
-27.643, -27.673, -27.697, -27.725, -27.753, -27.782, -27.823, -27.849, -27.876, -27.901, -27.931, -27.955,
-27.982, -28.009, -28.036, -28.065, -28.088, -28.117, -28.142, -28.172, -28.201, -28.232, -28.251, -28.278,
-28.304, -28.333, -28.361, -28.38, -28.403, -28.436, -28.464, -28.495, -28.531, -28.558, -28.581, -28.618,
-28.649, -28.679, -28.711, -28.74, -28.769, -28.792, -28.825, -28.851, -28.877, -28.901, -28.931, -28.955,
-28.984, -29.007, -29.039, -29.063, -29.097, -29.129, -29.158, -29.18, -29.206, -29.227, -29.251, -29.28,
-29.304, -29.328, -29.346, -29.369, -29.384, -29.409, -29.426, -29.458, -29.479, -29.499]
target_speed = 5 / 3.6 # [m/s]
T = 500.0 # 最大模拟时间
# 设置车辆的出事状态
state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
# state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
# print('x',x)
y = [state.y]
# print('y',y)
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while True:
# print('heading',tcp4.heading)
ai = PControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = update(state, ai, di)
time = time + dt
x.append(state.x)
x1 = x[-1]
print('x1', x[-1])
y.append(state.y)
y1 = y[-1]
print('y1', y[-1])
yaw.append(state.yaw)
yaw1 = y[-1] * 57.3
print('yaw', yaw[-1])
t.append(time)
# logger.add('file.log')
# logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
v.append(state.v)
# print('v', v[-1])
t.append(time)
# plt.cla()
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
# plt.axis("equal")
# plt.grid(True)
# plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
# plt.pause(0.001)
def car_back():
print('进入倒库')
while True:
global fangxp
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
a_trace = abs(math.sqrt(pow((float(tcp4.east)-68.35),2) + pow((float(tcp4.north)-43.85),2)))
print('a_trace',a_trace)
b_trace = abs(math.sqrt((float(tcp4.east)-70.15) ** 2 + (float(tcp4.north)-38.6) ** 2))
c_trace = abs(math.sqrt((float(tcp4.east)-70.7) ** 2 + (float(tcp4.north)-38.4) ** 2))
d_trace = abs(math.sqrt((float(tcp4.east)-72) ** 2 + (float(tcp4.north)-37.8) ** 2))
# print("tcp4.east",tcp4.east)
print('tcp_fangxiang',tcp4.fang_xiang)
if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and a_trace<1:
fangxp = -540
print('右打死',fangxp)
elif 70< float(tcp4.fang_xiang) < 110 and b_trace<1:
fangxp = 0
print('回正',fangxp)
elif 70 < float(tcp4.fang_xiang) < 110 and c_trace<1:
fangxp = -540
print('右打死2',fangxp)
elif 60< float(tcp4.fang_xiang) < 90 and d_trace<1:
fangxp = 0
print('回正2',fangxp)
else:
print('lose')
time.sleep(0.5)
if __name__ == '__main__':
# main()
t1 = threading.Thread(target=test5.can165) # 读取eps状态
t2 = threading.Thread(target=test5.can0a5) # 读取目前方向盘角度
t3 = threading.Thread(target=test5.main)
t4 = threading.Thread(target=test5.update)
t5 = threading.Thread(target=car_back)
t6 = threading.Thread(target=tcp4.zuo_biao)
t7 = threading.Thread(target=sped.sped)
# 启动线程
t5.start()
t6.start()
# t7.start()
# t1.start()
# t2.start()
# t3.start()
# t4.start()

547
pure_change_all.py
Unescape Escape View File

@ -1,547 +0,0 @@
"""
Path tracking simulation with pure pursuit steering control and PID speed control.
author: Atsushi Sakai (@Atsushi_twi)
"""
import datetime
import time
from loguru import logger
import math
import sqlite3
import threading
import time
import numpy as np
import geartest2
import pure_change_banck
import sped
import tcp4
import test5
k = 0 # look forward gain
Lfc = 5 # look-ahead distance
Kp = 1.0 # speed propotional gain
dt = 0.1 # [s]
L = 2.9 # [m] wheel base of vehicle
tx = 0
ty = 0
fangxp = 0
pure = 0
a_trace = 0
brk_open = 0
brk_change=0
e_trace=0
l_out_strat_trace=0
l_in_start_trace=0
right=0
left=0
# show_animation = True
#
class State:
def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a):
state.x = tcp4.x_car
# print('stax', state.x)
state.y = tcp4.y_car
# print('stay', state.y)
state.yaw = tcp4.car_yaw
# print('stayaw', state.car_yaw)
state.v = sped.speed + a * dt
# state.x = tcp4.x_car + sped.speed * math.cos(tcp4.car_yaw) * dt
# # print("state",state.x)
# state.y = tcp4.y_car + sped.speed * math.sin(tcp4.car_yaw) * dt
# state.car_yaw = tcp4.car_yaw + sped.speed / L * math.tan(delta) * dt
# state.v = sped.speed + a * dt
return state
# return state
def PIDControl(target, current):
a = Kp * (target - current)
return a
def pure_pursuit_control(state, cx, cy, pind):
global tx, ty, fangxp
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
ty = cy[ind]
else:
tx = cx[-1]
ty = cy[-1]
ind = len(cx) - 1
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
if delta > np.pi / 6.0:
delta = np.pi / 6.0
# print('delta1', delta)
elif delta < - np.pi / 6.0:
delta = - np.pi / 6.0
# print('delta2', delta)
angle = delta * 57.3
fangxp = int(angle * 18)
print('fxp', fangxp)
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
time.sleep(0.5)
return delta, ind
def calc_target_index(state, cx, cy):
# search nearest point index
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
L = 0.0
Lf = k * state.v + Lfc
# search look ahead target point index
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
return ind
def main():
global pure,e_trace
# target course
cx = [70.733, 70.703, 70.681, 70.661, 70.631, 70.608, 70.579, 70.554, 70.533, 70.509, 70.482, 70.457, 70.426,
70.402, 70.38, 70.352, 70.322, 70.298, 70.277, 70.253, 70.23, 70.205, 70.18, 70.151, 70.121, 70.093, 70.072,
70.046, 70.025, 70.0, 69.975, 69.95, 69.919, 69.895, 69.87, 69.84, 69.808, 69.778, 69.753, 69.722, 69.693,
69.668, 69.642, 69.605, 69.573, 69.543, 69.524, 69.5, 69.47, 69.442, 69.414, 69.389, 69.367, 69.344, 69.323,
69.297, 69.28, 69.256, 69.231, 69.21, 69.192, 69.166, 69.147, 69.127, 69.105, 69.087, 69.064, 69.04, 69.014,
68.987, 68.957, 68.926, 68.895, 68.866, 68.837, 68.804, 68.776, 68.746, 68.72, 68.696, 68.663, 68.636, 68.606,
68.573, 68.545, 68.519, 68.488, 68.46, 68.436, 68.406, 68.374, 68.343, 68.317, 68.29, 68.26, 68.228, 68.198,
68.17, 68.147, 68.121, 68.099, 68.075, 68.052, 68.024, 67.995, 67.969, 67.946, 67.912, 67.885, 67.864, 67.832,
67.799, 67.777, 67.748, 67.725, 67.689, 67.668, 67.64, 67.613, 67.584, 67.557, 67.532, 67.506, 67.475, 67.448,
67.415, 67.382, 67.347, 67.314, 67.287, 67.25, 67.221, 67.189, 67.165, 67.14, 67.12, 67.091, 67.064, 67.022,
66.999]
cy = [33.88, 33.985, 34.086, 34.188, 34.298, 34.409, 34.516, 34.633, 34.736, 34.847, 34.962, 35.077, 35.195, 35.318,
35.441, 35.563, 35.688, 35.812, 35.91, 36.011, 36.117, 36.227, 36.343, 36.506, 36.632, 36.758, 36.885, 37.013,
37.147, 37.275, 37.408, 37.542, 37.681, 37.818, 37.956, 38.1, 38.241, 38.382, 38.527, 38.666, 38.804, 38.95,
39.096, 39.24, 39.383, 39.526, 39.626, 39.765, 39.902, 40.033, 40.176, 40.31, 40.432, 40.561, 40.679, 40.793,
40.904, 41.011, 41.116, 41.215, 41.314, 41.442, 41.544, 41.649, 41.758, 41.867, 41.98, 42.1, 42.22, 42.345,
42.467, 42.592, 42.719, 42.839, 42.964, 43.079, 43.189, 43.305, 43.414, 43.517, 43.644, 43.744, 43.871,
43.996, 44.111, 44.229, 44.355, 44.462, 44.564, 44.672, 44.777, 44.88, 44.982, 45.083, 45.18, 45.302, 45.409,
45.505, 45.606, 45.713, 45.815, 45.919, 46.026, 46.127, 46.24, 46.348, 46.448, 46.565, 46.686, 46.786, 46.884,
47.015, 47.122, 47.221, 47.321, 47.448, 47.554, 47.66, 47.761, 47.862, 47.971, 48.082, 48.188, 48.295, 48.411,
48.525, 48.641, 48.76, 48.875, 48.991, 49.111, 49.218, 49.327, 49.433, 49.545, 49.645, 49.748, 49.845, 49.997,
50.097]
target_speed = 10.0 / 3.6 # [m/s]
# initial state
state = State(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while pure == 1:
ai = PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = update(state, ai)
time = time + dt
x.append(state.x)
x1 = round(x[-1], 3)
# print('x1', x[-1])
y.append(state.y)
y1 = round(y[-1], 3)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
print('纯跟踪运行')
e_trace = abs(math.sqrt(pow((float(tcp4.east) - 67.872), 2) + pow((float(tcp4.north) - 45.89), 2)))
print('e_trace', e_trace)
gear_d()
if e_trace < 1.2 and 160 < (float(tcp4.fang_xiang)) < 180:
pure = 2
print('退出纯跟踪')
shache()
print('刹车')
# logger.add(
# '预瞄点记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
# logger.debug('y {},x {},tx {}，ty {},fxp{},car_yaw{},北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(y1),
# str(x1), str(tx), str(ty), str(fangxp),
# str(state.yaw), str(tcp4.east), str(tcp4.north), str(tcp4.x_car),
# str(tcp4.y_car), str(tcp4.beidou[5]), str(tcp4.heading))
# if show_animation:
# plt.cla()
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
# plt.axis("equal")
# plt.grid(True)
# plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
# plt.pause(0.001)
#
# # Test
# assert lastIndex >= target_ind, "Cannot goal"
#
# if show_animation:
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.legend()
# plt.xlabel("x[m]")
# plt.ylabel("y[m]")
# plt.axis("equal")
# plt.grid(True)
#
# flg, ax = plt.subplots(1)
# plt.plot(t, [iv * 3.6 for iv in v], "-r")
# plt.xlabel("Time[s]")
# plt.ylabel("Speed[km/h]")
# plt.grid(True)
# plt.show()
# def date_write():
# shijian = datetime.datetime.now()
#
# logger.add(
# '预瞄点记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
# logger.debug('y {},x {},tx {}，ty {},fxp{},car_yaw{},北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(y1),
# str(x1), str(tx), str(ty), str(fangxp),
# str(state.yaw), str(tcp4.east), str(tcp4.north), str(tcp4.x_car),
# str(tcp4.y_car), str(tcp4.beidou[5]), str(tcp4.heading))
def shache():
global brk_open,brk_change
brk_open = 10
brk_change = 1
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set brk_open='%s',brk_change='%s' WHERE id = 1" % (brk_open, brk_change)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_p():
print('P')
gear_cmd = 1
auto_acc = 0
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def shache2():
brk_change=0
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set brk_change='%s' WHERE id = 1"%brk_change
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_r():
gear_cmd = 2 # r
print('r')
auto_acc = 70
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_d():
gear_cmd = 4 # r
# print('d')
auto_acc = 50
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def car_back():
global fangxp, a_trace, l_out_strat_trace,right
print('进入右倒库')
while right==1:
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
l_out_strat_trace=abs(math.sqrt(pow((float(tcp4.east) - 76.06), 2) + pow((float(tcp4.north) - 38.42), 2)))
print('l_out',l_out_strat_trace)
a_trace = abs(math.sqrt(pow((float(tcp4.east) - 68.35), 2) + pow((float(tcp4.north) - 43.85), 2)))
print('a_trace', a_trace)
b_trace = abs(math.sqrt((float(tcp4.east) - 70.15) ** 2 + (float(tcp4.north) - 38.6) ** 2))
c_trace = abs(math.sqrt((float(tcp4.east) - 70.7) ** 2 + (float(tcp4.north) - 38.4) ** 2))
d_trace = abs(math.sqrt((float(tcp4.east) - 72) ** 2 + (float(tcp4.north) - 37.8) ** 2))
# print("tcp4.east",tcp4.east)
print('tcp_fangxiang', tcp4.fang_xiang)
if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and a_trace < 0.7:
fangxp = -540
print('右打死', fangxp)
elif 70 < float(tcp4.fang_xiang) < 110 and b_trace < 1:
fangxp = 0
print('回正', fangxp)
elif 70 < float(tcp4.fang_xiang) < 110 and c_trace < 0.8:
fangxp = -540
print('右打死2', fangxp)
elif 60 < float(tcp4.fang_xiang) < 90 and d_trace < 1:
fangxp = 0
print('回正2', fangxp)
elif l_out_strat_trace<1.5 and 60< float(tcp4.fang_xiang)< 80:
right=2
print('右倒库结束')
# shache()
print('刹车')
else:
print('lose')
time.sleep(0.5)
def car_back_lelf_fly():
global fangxp,l_in_start_trace,left
print('进入左出库')
fangxp=0
while left==1:
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
l_in_start_trace=abs(math.sqrt(pow((float(tcp4.east)-72),2) + pow((float(tcp4.north)-28.54),2)))
print('l_in',l_in_start_trace)
lfa_trace = abs(math.sqrt(pow((float(tcp4.east)-72.02),2) + pow((float(tcp4.north)-37.51),2)))
print('a_trace',lfa_trace)
lfb_trace = abs(math.sqrt((float(tcp4.east)-71.66) ** 2 + (float(tcp4.north)-31.27)** 2))
# print('b_trace',b_trace)
# print("tcp4.east",tcp4.east)
print('tcp_fangxiang',tcp4.fang_xiang)
if 60.0 < (float(tcp4.fang_xiang)) < 100.0 and lfa_trace<1.2:
fangxp = 540
print('左打死',fangxp)
elif 330.0< float(tcp4.fang_xiang)< 370.0 and lfb_trace<1.2:
fangxp = 0
print('回正',fangxp)
elif l_in_start_trace<1 and 330< float(tcp4.fang_xiang)< 355:
left=2
print('开始左倒库')
# shache()
print('刹车')
else:
print('lose')
print('\n')
time.sleep(0.5)
def car_back_lelf():
global fangxp, l_in_start_trace
print('进入左倒库')
while True:
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
la_trace = abs(math.sqrt(pow((float(tcp4.east)-71.75),2) + pow((float(tcp4.north)-30.73),2)))
print('a_trace',la_trace)
lb_trace = abs(math.sqrt((float(tcp4.east)-71.06) ** 2 + (float(tcp4.north)-36)** 2))
print('b_trace',lb_trace)
lc_trace = abs(math.sqrt((float(tcp4.east)-71.88) ** 2 + (float(tcp4.north)-36.50) ** 2))
print('c_trace',lc_trace)
ld_trace = abs(math.sqrt((float(tcp4.east)-73.68) ** 2 + (float(tcp4.north)-38.10) ** 2))
print('d_trace',ld_trace)
# print("tcp4.east",tcp4.east)
print('tcp_fangxiang',tcp4.fang_xiang)
if 330.0 < (float(tcp4.fang_xiang)) < 370.0 and la_trace<1.2:
fangxp = 540
print('左打死',fangxp)
elif 40.0< float(tcp4.fang_xiang)< 80.0 and lb_trace<1.2:
fangxp = 0
print('回正',fangxp)
elif 60.0 < float(tcp4.fang_xiang) < 100.0 and lc_trace<1.5:
fangxp = 540
print('左打死',fangxp)
elif 70 < float(tcp4.fang_xiang) < 90 and ld_trace < 1.2:
fangxp = 0
print('回正',fangxp)
else:
print('lose')
print('\n')
time.sleep(0.5)
def car_back_comply():
global pure, brk_open,brk_change,e_trace,l_out_strat_trace,right,fangxp,left
# fangxp = 0
pure= 1
print('开始跟踪')
shache2()
main()
while True:
if e_trace < 1.2 and sped.speed == 0:
# gear_p()
brk_open = 0
# shache2()
# print('松刹车')
gear_r()
print('挂倒挡')
pure_change_banck.back = 1
pure_change_banck.main()
right=1
car_back()
if l_out_strat_trace<1.5 and 60< float(tcp4.fang_xiang)< 80:
print('右倒库结束，开始左飞')
left=1
# fangxp = 0
# gear_p()
brk_open = 0
shache2()
print('松刹车')
gear_d()
car_back_lelf_fly()
if l_in_start_trace<1 and 330< float(tcp4.fang_xiang)< 355:
print('开始左倒库')
# gear_p()
brk_open = 0
shache2()
print('松刹车')
gear_r()
car_back_lelf()
time.sleep(0.05)
if __name__ == '__main__':
t1 = threading.Thread(target=test5.can165) # 读取eps状态
t2 = threading.Thread(target=test5.can0a5) # 读取目前方向盘角度
t3 = threading.Thread(target=test5.main)
t4 = threading.Thread(target=test5.update)
t5 = threading.Thread(target=car_back_comply)
t6 = threading.Thread(target=tcp4.zuo_biao)
t7 = threading.Thread(target=sped.sped)
t8 = threading.Thread(target=geartest2.brk_open_option)
t9 = threading.Thread(target=geartest2.can4b8)
t10 = threading.Thread(target=geartest2.main)
t11 = threading.Thread(target=geartest2.update)
# 启动线程
t5.start()
t6.start()
t7.start()
t1.start()
t2.start()
t3.start()
t8.start()
t9.start()
t10.start()
t11.start()

393
pure_zhixian.py
Unescape Escape View File

@ -1,393 +0,0 @@
"""
Path tracking simulation with pure pursuit steering control and PID speed control.
author: Atsushi Sakai (@Atsushi_twi)
"""
import datetime
import time
from loguru import logger
import math
import sqlite3
import threading
import time
import numpy as np
import geartest2
import sped
import tcp4
import test5
k = 0 # look forward gain
Lfc = 5 # look-ahead distance
Kp = 1.0 # speed propotional gain
dt = 0.1 # [s]
L = 2.9 # [m] wheel base of vehicle
tx = 0
ty = 0
fangxp = 0
a = 0
a_trace = 0
brk_open = 0
brk_change=0
e_trace=0
# show_animation = True
#
class State:
def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a):
state.x = tcp4.x_car
# print('stax', state.x)
state.y = tcp4.y_car
# print('stay', state.y)
state.yaw = tcp4.car_yaw
# print('stayaw', state.car_yaw)
state.v = sped.speed + a * dt
# state.x = tcp4.x_car + sped.speed * math.cos(tcp4.car_yaw) * dt
# # print("state",state.x)
# state.y = tcp4.y_car + sped.speed * math.sin(tcp4.car_yaw) * dt
# state.car_yaw = tcp4.car_yaw + sped.speed / L * math.tan(delta) * dt
# state.v = sped.speed + a * dt
return state
# return state
def PIDControl(target, current):
a = Kp * (target - current)
return a
def pure_pursuit_control(state, cx, cy, pind):
global tx, ty, fangxp
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
ty = cy[ind]
else:
tx = cx[-1]
ty = cy[-1]
ind = len(cx) - 1
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
if delta > np.pi / 6.0:
delta = np.pi / 6.0
# print('delta1', delta)
elif delta < - np.pi / 6.0:
delta = - np.pi / 6.0
# print('delta2', delta)
angle = delta * 57.3
fangxp = int(angle * 18)
print('fxp', fangxp)
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
time.sleep(0.5)
return delta, ind
def calc_target_index(state, cx, cy):
# search nearest point index
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
L = 0.0
Lf = k * state.v + Lfc
# search look ahead target point index
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
return ind
def main():
global a,e_trace
# target course
cx = [70.733, 70.703, 70.681, 70.661, 70.631, 70.608, 70.579, 70.554, 70.533, 70.509, 70.482, 70.457, 70.426,
70.402, 70.38, 70.352, 70.322, 70.298, 70.277, 70.253, 70.23, 70.205, 70.18, 70.151, 70.121, 70.093, 70.072,
70.046, 70.025, 70.0, 69.975, 69.95, 69.919, 69.895, 69.87, 69.84, 69.808, 69.778, 69.753, 69.722, 69.693,
69.668, 69.642, 69.605, 69.573, 69.543, 69.524, 69.5, 69.47, 69.442, 69.414, 69.389, 69.367, 69.344, 69.323,
69.297, 69.28, 69.256, 69.231, 69.21, 69.192, 69.166, 69.147, 69.127, 69.105, 69.087, 69.064, 69.04, 69.014,
68.987, 68.957, 68.926, 68.895, 68.866, 68.837, 68.804, 68.776, 68.746, 68.72, 68.696, 68.663, 68.636, 68.606,
68.573, 68.545, 68.519, 68.488, 68.46, 68.436, 68.406, 68.374, 68.343, 68.317, 68.29, 68.26, 68.228, 68.198,
68.17, 68.147, 68.121, 68.099, 68.075, 68.052, 68.024, 67.995, 67.969, 67.946, 67.912, 67.885, 67.864, 67.832,
67.799, 67.777, 67.748, 67.725, 67.689, 67.668, 67.64, 67.613, 67.584, 67.557, 67.532, 67.506, 67.475, 67.448,
67.415, 67.382, 67.347, 67.314, 67.287, 67.25, 67.221, 67.189, 67.165, 67.14, 67.12, 67.091, 67.064, 67.022,
66.999]
cy = [33.88, 33.985, 34.086, 34.188, 34.298, 34.409, 34.516, 34.633, 34.736, 34.847, 34.962, 35.077, 35.195, 35.318,
35.441, 35.563, 35.688, 35.812, 35.91, 36.011, 36.117, 36.227, 36.343, 36.506, 36.632, 36.758, 36.885, 37.013,
37.147, 37.275, 37.408, 37.542, 37.681, 37.818, 37.956, 38.1, 38.241, 38.382, 38.527, 38.666, 38.804, 38.95,
39.096, 39.24, 39.383, 39.526, 39.626, 39.765, 39.902, 40.033, 40.176, 40.31, 40.432, 40.561, 40.679, 40.793,
40.904, 41.011, 41.116, 41.215, 41.314, 41.442, 41.544, 41.649, 41.758, 41.867, 41.98, 42.1, 42.22, 42.345,
42.467, 42.592, 42.719, 42.839, 42.964, 43.079, 43.189, 43.305, 43.414, 43.517, 43.644, 43.744, 43.871,
43.996, 44.111, 44.229, 44.355, 44.462, 44.564, 44.672, 44.777, 44.88, 44.982, 45.083, 45.18, 45.302, 45.409,
45.505, 45.606, 45.713, 45.815, 45.919, 46.026, 46.127, 46.24, 46.348, 46.448, 46.565, 46.686, 46.786, 46.884,
47.015, 47.122, 47.221, 47.321, 47.448, 47.554, 47.66, 47.761, 47.862, 47.971, 48.082, 48.188, 48.295, 48.411,
48.525, 48.641, 48.76, 48.875, 48.991, 49.111, 49.218, 49.327, 49.433, 49.545, 49.645, 49.748, 49.845, 49.997,
50.097]
target_speed = 10.0 / 3.6 # [m/s]
# initial state
state = State(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while a == 1:
ai = PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = update(state, ai)
time = time + dt
x.append(state.x)
x1 = round(x[-1], 3)
# print('x1', x[-1])
y.append(state.y)
y1 = round(y[-1], 3)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
shijian = datetime.datetime.now()
print('纯跟踪运行')
e_trace = abs(math.sqrt(pow((float(tcp4.east) - 67.872), 2) + pow((float(tcp4.north) - 45.89), 2)))
print('e_trace', e_trace)
gear_d()
if e_trace < 1 and 160 < (float(tcp4.fang_xiang)) < 180:
a = 2
print('退出纯跟踪')
shache()
print('刹车')
# logger.add(
# '预瞄点记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
# logger.debug('y {},x {},tx {}，ty {},fxp{},car_yaw{},北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(y1),
# str(x1), str(tx), str(ty), str(fangxp),
# str(state.yaw), str(tcp4.east), str(tcp4.north), str(tcp4.x_car),
# str(tcp4.y_car), str(tcp4.beidou[5]), str(tcp4.heading))
# if show_animation:
# plt.cla()
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
# plt.axis("equal")
# plt.grid(True)
# plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
# plt.pause(0.001)
#
# # Test
# assert lastIndex >= target_ind, "Cannot goal"
#
# if show_animation:
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.legend()
# plt.xlabel("x[m]")
# plt.ylabel("y[m]")
# plt.axis("equal")
# plt.grid(True)
#
# flg, ax = plt.subplots(1)
# plt.plot(t, [iv * 3.6 for iv in v], "-r")
# plt.xlabel("Time[s]")
# plt.ylabel("Speed[km/h]")
# plt.grid(True)
# plt.show()
def shache():
global brk_open,brk_change
brk_open = 10
brk_change = 1
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set brk_open='%s',brk_change='%s' WHERE id = 1" % (brk_open, brk_change)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_p():
print('P')
gear_cmd = 1
auto_acc = 0
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_r():
gear_cmd = 2 # r
print('r')
auto_acc = 70
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def gear_d():
gear_cmd = 4 # r
# print('d')
auto_acc = 48
db_file = "geardb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO geardb(id) VALUES(1);'
sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc)
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
def car_back():
print('进入倒库')
while True:
global fangxp, a_trace
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
a_trace = abs(math.sqrt(pow((float(tcp4.east) - 68.35), 2) + pow((float(tcp4.north) - 43.85), 2)))
print('a_trace', a_trace)
b_trace = abs(math.sqrt((float(tcp4.east) - 70.15) ** 2 + (float(tcp4.north) - 38.6) ** 2))
c_trace = abs(math.sqrt((float(tcp4.east) - 70.7) ** 2 + (float(tcp4.north) - 38.4) ** 2))
d_trace = abs(math.sqrt((float(tcp4.east) - 72) ** 2 + (float(tcp4.north) - 37.8) ** 2))
# print("tcp4.east",tcp4.east)
print('tcp_fangxiang', tcp4.fang_xiang)
if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and a_trace < 1:
fangxp = -540
print('右打死', fangxp)
elif 70 < float(tcp4.fang_xiang) < 110 and b_trace < 1:
fangxp = 0
print('回正', fangxp)
elif 70 < float(tcp4.fang_xiang) < 110 and c_trace < 1:
fangxp = -540
print('右打死2', fangxp)
elif 60 < float(tcp4.fang_xiang) < 90 and d_trace < 1.2:
fangxp = 0
print('回正2', fangxp)
else:
print('lose')
time.sleep(0.5)
def car_back_comply():
global a, brk_open,brk_change,e_trace
a = 1
print('开始跟踪')
main()
while True:
if e_trace < 1 and sped.speed == 0:
gear_p()
brk_open = 0
brk_change=0
print('松刹车')
gear_r()
print('挂倒挡')
car_back()
time.sleep(0.05)
if __name__ == '__main__':
t1 = threading.Thread(target=test5.can165) # 读取eps状态
t2 = threading.Thread(target=test5.can0a5) # 读取目前方向盘角度
t3 = threading.Thread(target=test5.main)
t4 = threading.Thread(target=test5.update)
t5 = threading.Thread(target=car_back_comply)
t6 = threading.Thread(target=tcp4.zuo_biao)
t7 = threading.Thread(target=sped.sped)
t8 = threading.Thread(target=geartest2.brk_open_option)
t9 = threading.Thread(target=geartest2.can4b8)
t10 = threading.Thread(target=geartest2.main)
t11 = threading.Thread(target=geartest2.update)
# 启动线程
t5.start()
t6.start()
t7.start()
t1.start()
t2.start()
t3.start()
t8.start()
t9.start()
t10.start()
t11.start()

369
pure_改进.py
Unescape Escape View File

@ -0,0 +1,369 @@
"""
Path tracking simulation with pure pursuit steering control and PID speed control.
author: Atsushi Sakai (@Atsushi_twi)
"""
import datetime
import time
from loguru import logger
import math
import sqlite3
import threading
import time
import numpy as np
import sped
import tcp4
import test5
k = 0 # look forward gain
Lfc = 5 # look-ahead distance
Kp = 1.0 # speed propotional gain
dt = 0.1 # [s]
L = 2.9 # [m] wheel base of vehicle
tx=0
ty=0
fangxp=0
# show_animation = True
#
class State:
def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a):
state.x = tcp4.x_car
# print('stax', state.x)
state.y = tcp4.y_car
# print('stay', state.y)
state.yaw = tcp4.car_yaw
# print('stayaw', state.car_yaw)
state.v = sped.speed + a * dt
# state.x = tcp4.x_car + sped.speed * math.cos(tcp4.car_yaw) * dt
# # print("state",state.x)
# state.y = tcp4.y_car + sped.speed * math.sin(tcp4.car_yaw) * dt
# state.car_yaw = tcp4.car_yaw + sped.speed / L * math.tan(delta) * dt
# state.v = sped.speed + a * dt
return state
# return state
def PIDControl(target, current):
a = Kp * (target - current)
return a
def pure_pursuit_control(state, cx, cy, pind):
global tx,ty,fangxp
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
ty = cy[ind]
else:
tx = cx[-1]
ty = cy[-1]
ind = len(cx) - 1
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
if delta > np.pi / 6.0:
delta = np.pi / 6.0
# print('delta1', delta)
elif delta < - np.pi / 6.0:
delta = - np.pi / 6.0
# print('delta2', delta)
angle = delta * 57.3
fangxp = int(angle * 18)
print('fxp', fangxp)
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
time.sleep(0.04)
return delta, ind
def calc_target_index(state, cx, cy):
# search nearest point index
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
L = 0.0
Lf = k * state.v + Lfc
# search look ahead target point index
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
return ind
def main():
# target course
cx = [84.61, 84.639, 84.667, 84.7, 84.722, 84.743, 84.775, 84.806, 84.837, 84.869, 84.9, 84.928, 84.949, 84.972,
85.004, 85.028, 85.06, 85.096, 85.123, 85.149, 85.177, 85.208, 85.24, 85.269, 85.298, 85.331, 85.361, 85.391,
85.423,
85.457, 85.488, 85.526, 85.547, 85.572, 85.602, 85.622, 85.648, 85.673, 85.694, 85.721, 85.741, 85.762,
85.786, 85.81, 85.832, 85.856, 85.88, 85.899, 85.923, 85.948, 85.973, 85.996, 86.017, 86.042, 86.068, 86.089,
86.113,
86.131,
86.154, 86.175, 86.195, 86.217, 86.245, 86.262, 86.288, 86.307, 86.33, 86.35, 86.376, 86.398, 86.423,
86.448, 86.474, 86.499, 86.52, 86.548, 86.575, 86.602, 86.624, 86.65, 86.676, 86.7, 86.72, 86.743, 86.77,
86.793, 86.821, 86.836, 86.861, 86.887, 86.916, 86.943, 86.964, 86.988, 87.01, 87.039, 87.066, 87.092, 87.12,
87.145,
87.169, 87.192, 87.221, 87.243, 87.271, 87.295, 87.324, 87.348, 87.369, 87.391, 87.417, 87.445, 87.475,
87.501, 87.525, 87.546, 87.575, 87.6, 87.629, 87.656, 87.685, 87.712, 87.736, 87.77, 87.794, 87.823, 87.852,
87.885,
87.916, 87.939, 87.963, 87.994, 88.019, 88.053, 88.084, 88.114, 88.138, 88.17, 88.196, 88.228, 88.257, 88.288,
88.318, 88.343, 88.373, 88.4, 88.427, 88.461, 88.489, 88.518, 88.548, 88.58, 88.606, 88.633, 88.66, 88.693,
88.712, 88.738, 88.765, 88.791, 88.826, 88.861, 88.888, 88.914, 88.943, 88.969, 88.996, 89.022, 89.052,
89.082,
89.113, 89.135, 89.158, 89.184, 89.218, 89.248, 89.272, 89.297, 89.334, 89.362, 89.39, 89.412, 89.445, 89.475,
89.504,
89.529, 89.561, 89.589, 89.617, 89.65, 89.673, 89.713, 89.736, 89.764, 89.792, 89.825, 89.853, 89.88, 89.913,
89.939, 89.968, 89.997, 90.026, 90.057, 90.086, 90.119, 90.144, 90.165, 90.194, 90.226, 90.251, 90.279,
90.306, 90.336, 90.365, 90.393, 90.424, 90.447, 90.477, 90.508, 90.533, 90.568, 90.601, 90.627, 90.654,
90.685,
90.713, 90.745, 90.776, 90.806, 90.835, 90.862, 90.894, 90.922, 90.949, 90.982, 91.009, 91.043, 91.069,
91.102,
91.138, 91.168, 91.199, 91.228, 91.26, 91.288, 91.316, 91.345, 91.372, 91.407, 91.436, 91.464, 91.494, 91.522,
91.552,
91.578, 91.603, 91.629, 91.66, 91.69, 91.713, 91.742, 91.774, 91.803, 91.823, 91.851, 91.877, 91.908, 91.935,
91.963, 91.993, 92.019, 92.045, 92.071, 92.096, 92.127, 92.154, 92.184, 92.207, 92.233, 92.264, 92.297,
92.324, 92.347, 92.374, 92.403, 92.43, 92.459, 92.487, 92.513, 92.544, 92.568, 92.596, 92.623, 92.652, 92.681,
92.706,
92.732, 92.762, 92.79, 92.826, 92.856, 92.881, 92.91, 92.936, 92.967, 92.991, 93.023, 93.047, 93.075, 93.105,
93.127, 93.155, 93.184, 93.214, 93.24, 93.263, 93.29, 93.32, 93.348, 93.376, 93.407, 93.433, 93.465, 93.498,
93.524, 93.559, 93.588, 93.621, 93.647, 93.675, 93.703, 93.735, 93.763, 93.797, 93.83, 93.858, 93.888, 93.914,
93.944, 93.975, 94.001, 94.026, 94.052, 94.083, 94.11, 94.141, 94.164, 94.188, 94.213, 94.24, 94.27, 94.296,
94.328, 94.355, 94.389, 94.418, 94.451, 94.48, 94.509, 94.535, 94.577, 94.607, 94.639, 94.672, 94.702, 94.724,
94.756, 94.786, 94.816, 94.847, 94.879, 94.907, 94.941, 94.97, 94.999, 95.028, 95.055, 95.089, 95.12, 95.149,
95.177, 95.215, 95.244, 95.275, 95.29, 95.32, 95.351, 95.379, 95.406, 95.435, 95.466, 95.493, 95.522, 95.546,
95.576, 95.612, 95.642, 95.674, 95.706, 95.732, 95.782, 95.812, 95.844, 95.872, 95.9, 95.925, 95.95, 95.985,
96.014, 96.048, 96.078, 96.109, 96.139, 96.17, 96.203, 96.234, 96.266, 96.299, 96.33, 96.364, 96.395, 96.424,
96.456, 96.487, 96.514, 96.546, 96.576, 96.609, 96.638, 96.667, 96.701, 96.735, 96.765, 96.795, 96.825,
96.855, 96.888, 96.918, 96.953, 96.982, 97.013, 97.045, 97.095, 97.123, 97.157, 97.179, 97.208, 97.243,
97.277,
97.308, 97.342, 97.376, 97.407, 97.435, 97.47, 97.501, 97.534, 97.565, 97.595, 97.624, 97.649, 97.684, 97.716,
97.749,
97.776, 97.811, 97.84, 97.869, 97.901, 97.931, 97.96, 97.984, 98.016, 98.042, 98.07, 98.1, 98.128, 98.155,
98.188, 98.218, 98.244, 98.273, 98.304, 98.339, 98.366, 98.397, 98.42, 98.465, 98.495, 98.523, 98.549, 98.578,
98.61,
98.637, 98.667, 98.7, 98.729, 98.757, 98.789, 98.819, 98.843, 98.872, 98.892, 98.921, 98.955, 98.984, 99.01,
99.04, 99.06, 99.09, 99.121, 99.152, 99.178, 99.208, 99.233, 99.26, 99.29, 99.32, 99.349, 99.376, 99.4,
99.428,
99.456, 99.486, 99.514, 99.544, 99.571, 99.598, 99.627, 99.654, 99.682, 99.714, 99.741, 99.769, 99.799,
99.831,
99.859, 99.888, 99.932, 99.957, 99.987, 100.012, 100.043, 100.076, 100.108, 100.135, 100.167, 100.191,
100.221,
100.252, 100.281, 100.311, 100.342, 100.371, 100.394, 100.427, 100.455, 100.49, 100.522, 100.553, 100.58,
100.614,
100.641, 100.672, 100.702, 100.728, 100.761, 100.792, 100.824, 100.849, 100.876, 100.909, 100.942, 100.967,
100.999,
101.031, 101.059, 101.09, 101.121, 101.15, 101.175, 101.204, 101.235, 101.264, 101.288, 101.32, 101.348,
101.38, 101.404, 101.433, 101.468, 101.495, 101.521, 101.551, 101.575, 101.604, 101.632, 101.66, 101.683,
101.716,
101.744, 101.776, 101.808, 101.83, 101.854, 101.889, 101.918, 101.945, 101.967, 101.99, 102.013, 102.033,
102.057,
102.082, 102.102, 102.116, 102.134, 102.149, 102.164, 102.176, 102.184, 102.187, 102.185, 102.183, 102.176,
102.165,
102.141, 102.112, 102.086, 102.058, 102.031, 101.989, 101.955, 101.912, 101.861, 101.805, 101.748, 101.68,
101.606, 101.522, 101.44, 101.353, 101.272, 101.185, 101.09, 100.999, 100.9, 100.804, 100.706, 100.611,
100.505,
100.406, 100.299, 100.195, 100.085, 99.979, 99.867, 99.739, 99.62, 99.493, 99.363, 99.231, 99.102, 98.965,
98.83,
98.623, 98.48, 98.339, 98.198, 98.06, 97.927, 97.794, 97.659, 97.534, 97.402, 97.268, 97.143, 97.023, 96.896,
96.773,
96.645, 96.518, 96.393, 96.26, 96.127, 95.988, 95.845, 95.706, 95.558, 95.409, 95.258, 95.102, 94.945, 94.788,
94.633, 94.475, 94.315, 94.16, 94.002, 93.903, 93.75, 93.589, 93.434, 93.274, 93.118, 92.963, 92.807, 92.648,
92.487, 92.333, 92.18, 92.022, 91.864, 91.711, 91.553, 91.407, 91.252, 91.102, 90.954, 90.806, 90.652, 90.511,
90.354, 90.203, 90.049, 89.89, 89.737, 89.581, 89.422, 89.26, 89.108, 88.949, 88.793, 88.636, 88.48, 88.326,
88.168, 88.01, 87.855, 87.7, 87.548, 87.395, 87.243, 87.088, 86.939, 86.785, 86.635, 86.482, 86.334, 86.185,
86.038, 85.886, 85.666, 85.516, 85.367, 85.217, 85.067, 84.913, 84.766, 84.62, 84.473, 84.322, 84.171, 84.022,
83.871, 83.722, 83.576, 83.429, 83.282, 83.132, 82.981, 82.834, 82.687, 82.524, 82.375, 82.225, 82.072,
81.918, 81.772, 81.618, 81.457, 81.309, 81.154, 80.998, 80.843, 80.694, 80.543, 80.385, 80.237, 80.086,
79.937,
79.785, 79.639, 79.493, 79.355, 79.205, 79.063, 78.922, 78.784, 78.647, 78.517, 78.384, 78.25, 78.119, 77.986,
77.856,
77.741, 77.625, 77.511, 77.403, 77.298, 77.199, 77.065, 76.938, 76.826, 76.707]
cy = [66.014, 65.909, 65.805, 65.693, 65.595, 65.496, 65.384, 65.267, 65.144, 65.021, 64.894, 64.796, 64.695,
64.595, 64.484, 64.373, 64.26, 64.141, 64.023, 63.9, 63.776, 63.65, 63.521, 63.395, 63.261, 63.128, 62.997,
62.864, 62.734, 62.595, 62.453, 62.312, 62.214, 62.114, 61.97, 61.87, 61.769, 61.667, 61.565, 61.464, 61.362,
61.257, 61.157, 61.05, 60.944, 60.842, 60.735, 60.63, 60.523, 60.414, 60.306, 60.198, 60.091, 59.98, 59.867,
59.755, 59.645, 59.535, 59.419, 59.308, 59.196, 59.088, 58.971, 58.855, 58.736, 58.616, 58.494, 58.373,
58.253, 58.133, 58.012, 57.892, 57.767, 57.645, 57.525, 57.404, 57.282, 57.163, 57.041, 56.922, 56.801,
56.682, 56.562, 56.442, 56.323, 56.207, 56.081, 55.967, 55.835, 55.715, 55.585, 55.454, 55.327, 55.196, 55.07,
54.942, 54.814, 54.69, 54.572, 54.443, 54.317, 54.191, 54.067, 53.949, 53.83, 53.713, 53.603, 53.483, 53.364,
53.244, 53.126, 53.007, 52.882, 52.76, 52.634, 52.507, 52.386, 52.26, 52.133, 52.002, 51.873, 51.741, 51.607,
51.472, 51.34, 51.205, 51.072, 50.934, 50.794, 50.655, 50.512, 50.376, 50.235, 50.095, 49.951, 49.808, 49.666,
49.522, 49.381, 49.237, 49.094, 48.955, 48.814, 48.678, 48.541, 48.403, 48.254, 48.045, 47.903, 47.762,
47.626, 47.483, 47.337, 47.193, 47.049, 46.903, 46.759, 46.613, 46.465, 46.313, 46.168, 46.027, 45.881, 45.74,
45.599, 45.457, 45.309, 45.166, 45.021, 44.875, 44.729, 44.582, 44.439, 44.292, 44.143, 43.997, 43.851,
43.709, 43.568, 43.426, 43.279, 43.136, 42.994, 42.849, 42.704, 42.561, 42.418, 42.273, 42.127, 41.986,
41.844, 41.7, 41.552, 41.404, 41.257, 41.112, 40.964, 40.818, 40.672, 40.526, 40.381, 40.236, 40.09, 39.941,
39.797, 39.653, 39.512, 39.362, 39.223, 39.08, 38.94, 38.795, 38.653, 38.508, 38.368, 38.226, 38.078, 37.931,
37.788, 37.639, 37.496, 37.351, 37.206, 37.059, 36.908, 36.763, 36.613, 36.463, 36.319, 36.172, 36.024,
35.877, 35.729, 35.58, 35.434, 35.286, 35.139, 34.995, 34.844, 34.695, 34.552, 34.402, 34.256, 34.107, 33.958,
33.813, 33.663, 33.512, 33.363, 33.211, 33.067, 32.922, 32.773, 32.627, 32.484, 32.336, 32.19, 32.047, 31.903,
31.76, 31.61, 31.469, 31.325, 31.183, 31.039, 30.901, 30.762, 30.625, 30.488, 30.349, 30.213, 30.074, 29.935,
29.797, 29.661, 29.522, 29.393, 29.254, 29.102, 28.959, 28.815, 28.671, 28.524, 28.383, 28.241, 28.104,
27.968, 27.823, 27.69, 27.55, 27.409, 27.267, 27.128, 26.985, 26.842, 26.698, 26.554, 26.406, 26.258, 26.107,
25.96, 25.808, 25.664, 25.513, 25.366, 25.215, 25.069, 24.927, 24.791, 24.654, 24.513, 24.374, 24.234, 24.098,
23.954, 23.812, 23.664, 23.521, 23.375, 23.228, 23.083, 22.934, 22.789, 22.645, 22.499, 22.351, 22.205,
22.057, 21.913, 21.767, 21.628, 21.484, 21.337, 21.191, 21.046, 20.9, 20.758, 20.618, 20.476, 20.33, 20.191,
20.049, 19.908, 19.766, 19.632, 19.489, 19.353, 19.217, 19.076, 18.935, 18.797, 18.659, 18.516, 18.374,
18.227, 18.085, 17.935, 17.789, 17.64, 17.495, 17.344, 17.119, 16.969, 16.823, 16.669, 16.521, 16.371, 16.219,
16.072, 15.926, 15.775, 15.629, 15.483, 15.341, 15.195, 15.05, 14.901, 14.757, 14.61, 14.463, 14.315, 14.167,
14.023, 13.871, 13.726, 13.582, 13.436, 13.291, 13.147, 13.007, 12.863, 12.725, 12.586, 12.446, 12.31, 12.175,
12.043, 11.898, 11.76, 11.621, 11.473, 11.259, 11.114, 10.97, 10.823, 10.671, 10.523, 10.375, 10.233, 10.082,
9.935, 9.788, 9.642, 9.494, 9.342, 9.194, 9.046, 8.896, 8.747, 8.598, 8.451, 8.302, 8.158, 8.012, 7.864,
7.718, 7.569, 7.425, 7.28, 7.132, 6.986, 6.841, 6.693, 6.543, 6.396, 6.246, 6.099, 5.95, 5.801, 5.653, 5.5,
5.351, 5.2, 4.974, 4.821, 4.669, 4.522, 4.373, 4.222, 4.068, 3.918, 3.77, 3.623, 3.472, 3.322, 3.173, 3.025,
2.882, 2.733, 2.583, 2.436, 2.289, 2.144, 1.994, 1.845, 1.695, 1.546, 1.397, 1.245, 1.091, 0.939, 0.791,
0.641, 0.493, 0.346, 0.197, 0.051, -0.102, -0.254, -0.404, -0.553, -0.708, -0.859, -1.012, -1.166, -1.321,
-1.474, -1.628, -1.858, -2.012, -2.164, -2.32, -2.473, -2.626, -2.776, -2.927, -3.078, -3.231, -3.384, -3.535,
-3.686, -3.836, -3.987, -4.136, -4.287, -4.43, -4.58, -4.728, -4.872, -5.013, -5.158, -5.302, -5.442, -5.585,
-5.728, -5.873, -6.015, -6.158, -6.305, -6.447, -6.59, -6.733, -6.877, -7.022, -7.165, -7.307, -7.449, -7.59,
-7.732, -7.871, -8.012, -8.151, -8.29, -8.429, -8.576, -8.718, -8.863, -9.006, -9.153, -9.371, -9.514, -9.657,
-9.789, -9.937, -10.083, -10.229, -10.378, -10.523, -10.668, -10.812, -10.958, -11.103, -11.247, -11.395,
-11.54, -11.689, -11.832, -11.978, -12.121, -12.267, -12.412, -12.559, -12.705, -12.85, -13.006, -13.15,
-13.3, -13.447, -13.593, -13.741, -13.889, -14.033, -14.182, -14.326, -14.474, -14.607, -14.754, -14.903,
-15.051, -15.195, -15.346, -15.491, -15.642, -15.794, -15.945, -16.091, -16.249, -16.401, -16.546, -16.696,
-16.843, -16.986, -17.132, -17.272, -17.413, -17.553, -17.694, -17.839, -17.984, -18.128, -18.271, -18.413,
-18.558, -18.705, -18.857, -19.008, -19.149, -19.293, -19.441, -19.586, -19.73, -19.876, -20.023, -20.169,
-20.317, -20.462, -20.61, -20.763, -20.912, -21.067, -21.225, -21.373, -21.528, -21.675, -21.818, -21.953,
-22.076, -22.203, -22.372, -22.481, -22.601, -22.718, -22.832, -22.95, -23.07, -23.192, -23.315, -23.439,
-23.56, -23.68, -23.799, -23.912, -24.022, -24.132, -24.242, -24.34, -24.435, -24.518, -24.605, -24.686,
-24.761, -24.838, -24.911, -24.978, -25.039, -25.1, -25.154, -25.213, -25.253, -25.307, -25.361, -25.408,
-25.451, -25.493, -25.53, -25.573, -25.62, -25.642, -25.665, -25.683, -25.703, -25.718, -25.732, -25.747,
-25.758, -25.759, -25.769, -25.778, -25.789, -25.799, -25.799, -25.8, -25.8, -25.8, -25.8, -25.807, -25.816,
-25.823, -25.836, -25.841, -25.861, -25.884, -25.908, -25.936, -25.961, -25.99, -26.017, -26.048, -26.084,
-26.118, -26.141, -26.176, -26.213, -26.247, -26.274, -26.299, -26.34, -26.373, -26.403, -26.435, -26.467,
-26.5, -26.532, -26.563, -26.599, -26.624, -26.661, -26.683, -26.715, -26.748, -26.78, -26.813, -26.842,
-26.873, -26.908, -26.943, -26.977, -27.008, -27.043, -27.068, -27.095, -27.133, -27.164, -27.193, -27.224,
-27.255, -27.287, -27.319, -27.348, -27.381, -27.407, -27.448, -27.483, -27.518, -27.55, -27.587, -27.615,
-27.643, -27.673, -27.697, -27.725, -27.753, -27.782, -27.823, -27.849, -27.876, -27.901, -27.931, -27.955,
-27.982, -28.009, -28.036, -28.065, -28.088, -28.117, -28.142, -28.172, -28.201, -28.232, -28.251, -28.278,
-28.304, -28.333, -28.361, -28.38, -28.403, -28.436, -28.464, -28.495, -28.531, -28.558, -28.581, -28.618,
-28.649, -28.679, -28.711, -28.74, -28.769, -28.792, -28.825, -28.851, -28.877, -28.901, -28.931, -28.955,
-28.984, -29.007, -29.039, -29.063, -29.097, -29.129, -29.158, -29.18, -29.206, -29.227, -29.251, -29.28,
-29.304, -29.328, -29.346, -29.369, -29.384, -29.409, -29.426, -29.458, -29.479, -29.499]
target_speed = 10.0 / 3.6 # [m/s]
# initial state
state = State(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while True:
ai = PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = update(state, ai)
time = time + dt
x.append(state.x)
x1 = round(x[-1], 3)
# print('x1', x[-1])
y.append(state.y)
y1 = round(y[-1], 3)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
shijian = datetime.datetime.now()
# logger.add(
# '预瞄点记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
# logger.debug('y {},x {},tx {}，ty {},fxp{},car_yaw{},北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(y1),
# str(x1), str(tx), str(ty), str(fangxp),
# str(state.yaw), str(tcp4.east), str(tcp4.north), str(tcp4.x_car),
# str(tcp4.y_car), str(tcp4.beidou[5]), str(tcp4.heading))
# if show_animation:
# plt.cla()
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
# plt.axis("equal")
# plt.grid(True)
# plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
# plt.pause(0.001)
#
# # Test
# assert lastIndex >= target_ind, "Cannot goal"
#
# if show_animation:
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.legend()
# plt.xlabel("x[m]")
# plt.ylabel("y[m]")
# plt.axis("equal")
# plt.grid(True)
#
# flg, ax = plt.subplots(1)
# plt.plot(t, [iv * 3.6 for iv in v], "-r")
# plt.xlabel("Time[s]")
# plt.ylabel("Speed[km/h]")
# plt.grid(True)
# plt.show()
if __name__ == '__main__':
t1 = threading.Thread(target=test5.can165) # 读取eps状态
t2 = threading.Thread(target=test5.can0a5) # 读取目前方向盘角度
t3 = threading.Thread(target=test5.main)
t4 = threading.Thread(target=test5.update)
t5 = threading.Thread(target=main)
t6 = threading.Thread(target=tcp4.zuo_biao)
t7 = threading.Thread(target=sped.sped)
# 启动线程
t5.start()
t6.start()
t7.start()
t1.start()
t2.start()
t3.start()
Write Preview
Loading…
Cancel
Save