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

Compare commits

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

No commits in common. 'a97d7f47e990df83d623679e8446288a2b56cd62' and 'master' have entirely different histories.
a97d7f47e9 ... master

19 changed files with 2352 additions and 355 deletions
Show Stats

3
.idea/.gitignore vendored
Unescape Escape View File

@ -0,0 +1,3 @@
# 默认忽略的文件
/shelf/
/workspace.xml

23
.idea/inspectionProfiles/Project_Default.xml
Unescape Escape View File

@ -0,0 +1,23 @@
<component name="InspectionProjectProfileManager">
<profile version="1.0">
<option name="myName" value="Project Default" />
<inspection_tool class="PyPep8NamingInspection" enabled="true" level="WEAK WARNING" enabled_by_default="true">
<option name="ignoredErrors">
<list>
<option value="N801" />
<option value="N806" />
<option value="N802" />
</list>
</option>
</inspection_tool>
<inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
<option name="ignoredIdentifiers">
<list>
<option value="float.*" />
<option value="int.__getitem__" />
<option value="int.*" />
</list>
</option>
</inspection_tool>
</profile>
</component>

6
.idea/inspectionProfiles/profiles_settings.xml
Unescape Escape View File

@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

4
.idea/misc.xml
Unescape Escape View File

@ -0,0 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8 (pycharm) (2)" project-jdk-type="Python SDK" />
</project>

8
.idea/modules.xml
Unescape Escape View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/pure_自适应.iml" filepath="$PROJECT_DIR$/.idea/pure_自适应.iml" />
</modules>
</component>
</project>

8
.idea/pure_自适应.iml
Unescape Escape View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="Python 3.8 (pycharm) (2)" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
</module>

8
.idea/toolchains.xml
Unescape Escape View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="NimToolchainService">
<option name="rootPaths">
<list />
</option>
</component>
</project>

6
.idea/vcs.xml
Unescape Escape View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

7
1132.py
Unescape Escape View File

@ -1,7 +0,0 @@
# NC=30
# for p in range(NC):
# print('p', p)
# for q in range(p + 1):
# print('q', q)

272
MPC.py
Unescape Escape View File

@ -1,272 +0,0 @@
import time
import matplotlib.pyplot as plt
import numpy as np
from math import *
from cvxopt import matrix, solvers
class MPC:
def __init__(self):
self.Np = 60 # 预测步长
self.Nc = 30 # 控制步长
self.dt = 0.1 # 时间间隔
self.Length = 1.0 # 车辆轴距
max_steer = 30 * pi / 180 # 最大方向盘打角
max_steer_v = 15 * pi / 180 # 最大方向盘打角速度
max_v = 8.7 # 最大车速
max_a = 1.0 # 最大加速度
# 目标函数相关矩阵
self.Q =150* np.identity(3 * self.Np) # 位姿权重 100为权重
print('S',self.Q)
self.R = 100 * np.identity(2 * self.Nc) # 控制权重
print('r',self.R)
self.kesi = np.zeros((5, 1))
print('kesi',self.kesi)
self.A = np.identity(5)
print('A',self.A)
self.B = np.block([
[np.zeros((3, 2))],
[np.identity(2)]
])
print('B',self.B)
self.C = np.block([
[np.identity(3), np.zeros((3, 2))]
])
print(self.C)
self.PHI = np.zeros((3 * self.Np, 5))
self.THETA = np.zeros((3 * self.Np, 2 * self.Nc))
self.CA = (self.Np + 1) * [self.C]
# print(self.CA)
self.H = np.zeros((2 * self.Nc, 2 * self.Nc))
self.f = np.zeros((2 * self.Nc, 1))
# 不等式约束相关矩阵
A_t = np.zeros((self.Nc, self.Nc))
for p in range(self.Nc):
# print('p',p)
for q in range(p + 1):
# print('q', q)
A_t[p][q] = 1
A_I = np.kron(A_t, np.identity(2))
# print(A_I)
# 控制量约束
umin = np.array([[-max_v], [-max_steer]])
print(umin)
umax = np.array([[max_v], [max_steer]])
self.Umin = np.kron(np.ones((self.Nc, 1)), umin)
self.Umax = np.kron(np.ones((self.Nc, 1)), umax)
# 控制增量约束
delta_umin = np.array([[-max_a * self.dt], [-max_steer_v * self.dt]])
delta_umax = np.array([[max_a * self.dt], [max_steer_v * self.dt]])
delta_Umin = np.kron(np.ones((self.Nc, 1)), delta_umin)
delta_Umax = np.kron(np.ones((self.Nc, 1)), delta_umax)
self.A_cons = np.zeros((2 * 2 * self.Nc, 2 * self.Nc))
self.A_cons[0:2 * self.Nc, 0:2 * self.Nc] = A_I
self.A_cons[2 * self.Nc:4 * self.Nc, 0:2 * self.Nc] = np.identity(2 * self.Nc)
self.lb_cons = np.zeros((2 * 2 * self.Nc, 1))
self.lb_cons[2 * self.Nc:4 * self.Nc, 0:1] = delta_Umin
self.ub_cons = np.zeros((2 * 2 * self.Nc, 1))
self.ub_cons[2 * self.Nc:4 * self.Nc, 0:1] = delta_Umax
def mpcControl(self, x, y, yaw, v, angle, tar_x, tar_y, tar_yaw, tar_v, tar_angle): # mpc优化控制
T = self.dt
L = self.Length
# 更新误差
self.kesi[0][0] = x - tar_x
print('1,0',self.kesi[0][0])
self.kesi[1][0] = y - tar_y
self.kesi[2][0] = self.normalizeTheta(yaw - tar_yaw)
self.kesi[3][0] = v - tar_v
self.kesi[4][0] = angle - tar_angle
# 更新A矩阵
self.A[0][2] = -tar_v * sin(tar_yaw) * T
self.A[0][3] = cos(tar_yaw) * T
self.A[1][2] = tar_v * cos(tar_yaw) * T
self.A[1][3] = sin(tar_yaw) * T
self.A[2][3] = tan(tar_angle) * T / L
self.A[2][4] = tar_v * T / (L * (cos(tar_angle) ** 2))
# 更新B矩阵
self.B[0][0] = cos(tar_yaw) * T
self.B[1][0] = sin(tar_yaw) * T
self.B[2][0] = tan(tar_angle) * T / L
self.B[2][1] = tar_v * T / (L * (cos(tar_angle) ** 2))
# 更新CA
for i in range(1, self.Np + 1):
self.CA[i] = np.dot(self.CA[i - 1], self.A)
# 更新PHI和THETA
for j in range(self.Np):
self.PHI[3 * j:3 * (j + 1), 0:5] = self.CA[j + 1]
for k in range(min(self.Nc, j + 1)):
self.THETA[3 * j:3 * (j + 1), 2 * k: 2 * (k + 1)
] = np.dot(self.CA[j - k], self.B)
# 更新H
self.H = np.dot(np.dot(self.THETA.transpose(), self.Q),
self.THETA) + self.R
# 更新f
self.f = 2 * np.dot(np.dot(self.THETA.transpose(), self.Q),
np.dot(self.PHI, self.kesi))
# 更新约束
Ut = np.kron(np.ones((self.Nc, 1)), np.array([[v], [angle]]))
self.lb_cons[0:2 * self.Nc, 0:1] = self.Umin - Ut
self.ub_cons[0:2 * self.Nc, 0:1] = self.Umax - Ut
# 求解QP
P = matrix(self.H)
q = matrix(self.f)
G = matrix(np.block([
[self.A_cons],
[-self.A_cons]
]))
h = matrix(np.block([
[self.ub_cons],
[-self.lb_cons]
]))
solvers.options['show_progress'] = False
sol = solvers.qp(P, q, G, h)
X = sol['x']
# 输出结果
v += X[0]
angle += X[1]
return v, angle
def normalizeTheta(self, angle): # 角度归一化
while (angle >= pi):
angle -= 2 * pi
while (angle < -pi):
angle += 2 * pi
return angle
def findIdx(self, x, y, cx, cy): # 寻找欧式距离最近的点
min_dis = float('inf')
idx = 0
for i in range(len(cx)):
dx = x - cx[i]
dy = y - cy[i]
dis = dx ** 2 + dy ** 2
if (dis < min_dis):
min_dis = dis
idx = i
return idx
def update(self, x, y, yaw, v, angle): # 模拟车辆位置
x += v * cos(yaw) * self.dt
y += v * sin(yaw) * self.dt
yaw += v / self.Length * tan(angle) * self.dt
return x, y, yaw
if __name__ == '__main__':
cx = np.linspace(0, 200, 2000)
cy = np.zeros(len(cx))
dx = np.zeros(len(cx))
ddx = np.zeros(len(cy))
cyaw = np.zeros(len(cx))
ck = np.zeros(len(cx))
for i in range(len(cx)):
cy[i] = cos(cx[i] / 10) * cx[i] / 10
# 计算一阶导数
for i in range(len(cx) - 1):
dx[i] = (cy[i + 1] - cy[i]) / (cx[i + 1] - cx[i])
dx[len(cx) - 1] = dx[len(cx) - 2]
# 计算二阶导数
for i in range(len(cx) - 2):
ddx[i] = (cy[i + 2] - 2 * cy[i + 1] + cy[i]) / (0.5 * (cx[i + 2] - cx[i])) ** 2
ddx[len(cx) - 2] = ddx[len(cx) - 3]
ddx[len(cx) - 1] = ddx[len(cx) - 2]
# 计算偏航角
for i in range(len(cx)):
cyaw[i] = atan(dx[i])
# 计算曲率
for i in range(len(cx)):
ck[i] = ddx[i] / (1 + dx[i] ** 2) ** 1.5
# 初始状态
x = 0.0
y = 5.0
yaw = 0.0
v = 0.0
angle = 0.0
t = 0
# 历史状态
xs = [x]
ys = [y]
vs = [v]
angles = [angle]
ts = [t]
# 实例化
mpc = MPC()
while (1):
idx = mpc.findIdx(x, y, cx, cy)
if (idx == len(cx) - 1):
break
tar_v = 80 / 3.6
tar_angle = atan(mpc.Length * ck[idx])
(v, angle) = mpc.mpcControl(x, y, yaw, v, angle,
cx[idx], cy[idx], cyaw[idx], tar_v, tar_angle)
(x, y, yaw) = mpc.update(x, y, yaw, v, angle)
# 保存状态
xs.append(x)
ys.append(y)
vs.append(v)
angles.append(angle)
t = t + 0.1
ts.append(t)
# 显示
plt.plot(cx, cy)
plt.scatter(xs, ys, c='r', marker='*')
plt.pause(0.01) # 暂停0.01秒
plt.clf()
# plt.close()
# plt.subplot(2, 1, 1)
# plt.plot(ts, vs)
# plt.subplot(2, 1, 2)
# plt.plot(ts, angles)
# plt.show()

76
MPc2.py
Unescape Escape View File

@ -1,76 +0,0 @@
import numpy as np
import scipy.linalg
from cvxopt import solvers, matrix
import matplotlib.pyplot as plt
A = np.array([[1, 1], [-1, 2]])
n = A.shape[0]
B = np.array([[1, 1], [1, 2]])
p = B.shape[1]
Q = np.array([[1, 0], [0, 1]])
F = np.array([[1, 0], [0, 1]])
R = np.array([[1, 0], [0, 0.1]])
k_steps = 100
X_k = np.zeros((n, k_steps))
X_k[:, 0] = [10, -10]
U_k = np.zeros((p, k_steps))
N = 5
def cal_matrices(A, B, Q, R, F, N):
n = A.shape[0]
p = B.shape[1]
M = np.vstack((np.eye((n)), np.zeros((N * n, n))))
C = np.zeros(((N + 1) * n, N * p))
tmp = np.eye(n)
for i in range(N):
rows = i * n + n
C[rows:rows + n, :] = np.hstack((np.dot(tmp, B), C[rows - n:rows, 0:(N - 1) * p]))
tmp = np.dot(A, tmp)
M[rows:rows + n, :] = tmp
Q_bar_be = np.kron(np.eye(N), Q)
Q_bar = scipy.linalg.block_diag(Q_bar_be, F)
R_bar = np.kron(np.eye(N), R)
G = np.matmul(np.matmul(M.transpose(), Q_bar), M)
E = np.matmul(np.matmul(C.transpose(), Q_bar), M)
H = np.matmul(np.matmul(C.transpose(), Q_bar), C) + R_bar
return H, E
def Prediction(M, T):
sol = solvers.qp(M, T)
U_thk = np.array(sol["x"])
u_k = U_thk[0:2, :]
return u_k
M, C = cal_matrices(A, B, Q, R, F, N)
M = matrix(M)
for k in range(1, k_steps):
x_kshort = X_k[:, k - 1].reshape(2, 1)
u_kshort = U_k[:, k - 1].reshape(2, 1)
T = np.dot(C, x_kshort)
T = matrix(T)
for i in range(2):
U_k[i:, k - 1] = Prediction(M, T)[i, 0]
X_knew = np.dot(A, x_kshort) + np.dot(B, u_kshort)
for j in range(2):
X_k[j:, k] = X_knew[j, 0]
print(X_k)

48
curvature.py
Unescape Escape View File

@ -0,0 +1,48 @@
import math
A = [100.012, -9.789]
C = [100.076, -10.083]
B = [100.043, -9.937]
s = abs((A[0] * B[1] + B[0] * C[1] + C[0] * A[1] - A[1] * B[0] - B[1] * C[0] - C[1] * 1) / 2)
print('s',s)
a = abs(math.sqrt(abs(A[0] - C[0]) ** 2) + abs((A[1] - C[1]) ** 2))
print('a',a)
b = abs(math.sqrt(abs(B[0] - C[0]) ** 2) + abs((B[1] - C[1]) ** 2))
print('b',b)
c = abs(math.sqrt(a ** 2 + b ** 2))
print('c',c)
k = 4 * s / (a * b * c)
print(k)
# import numpy as np
# def get_arc_curve(pts):
# '''
# 获取弧度值
# :param pts:
# :return:
# '''
#
# # 计算弦长
# start = np.array(pts[0])
# end = np.array(pts[len(pts) - 1])
# l_arc = np.sqrt(np.sum(np.power(end - start, 2)))
#
# # 计算弧上的点到直线的最大距离
# # 计算公式：\frac{1}{2a}\sqrt{(a+b+c)(a+b-c)(a+c-b)(b+c-a)}
# a = l_arc
# b = np.sqrt(np.sum(np.power(pts - start, 2), axis=1))
# c = np.sqrt(np.sum(np.power(pts - end, 2), axis=1))
# dist = np.sqrt((a + b + c) * (a + b - c) * (a + c - b) * (b + c - a)) / (2 * a)
# h = dist.max()
#
# # 计算曲率
# r = ((a * a) / 4 + h * h) / (2 * h)
#
# return r
#
#
# if __name__ == '__main__':
# x = np.linspace(1, 100, 99).astype(np.int64)
# y = (x ** 2)
# xy = list(zip(x, y)) # list of points in 2D space
# print(get_arc_curve(xy))

134
gear_change.py
Unescape Escape View File

@ -0,0 +1,134 @@
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

@ -0,0 +1,235 @@
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()

379
pure_CUR_last.py
Unescape Escape View File

@ -0,0 +1,379 @@
import time
# import txt2
import numpy as np
import math
import matplotlib.pyplot as plt
from loguru import logger
# import tcp4
import sqlite3
import threading
import CUR
ag = [0,0]
a = []
# import test5
k = 0 # 前视距离系数
Lfc =3# 前视距离
Kp = 1.0 # 速度P控制器系数
dt = 0.1 # 时间间隔，单位：s
L = 2.7 # 车辆轴距，单位：m
x1=[]
y1=[]
# yaw1 = math.radians(414)
# print('y',yaw1)
# arc=math.atan2(1, 1)
# print('acr',arc)
yaw = math.radians(450 - (344 + 180 - 360))
f=0
T = []
TX = []
curvature=0
A=0
B=0
C=0
def calculate_curvature(point_a, point_b, point_c):
global curvature
# 将输入点转换为 NumPy 数组
a = np.array(point_a)
b = np.array(point_b)
c = np.array(point_c)
# 计算各边长
ab = np.linalg.norm(a - b)
bc = np.linalg.norm(b - c)
ca = np.linalg.norm(c - a)
# 使用海伦公式计算三角形面积
s = (ab + bc + ca) / 2
area = np.sqrt(s * (s - ab) * (s - bc) * (s - ca))
# 计算曲率
curvature = 4 * area / (ab * bc * ca)
print('cur',curvature)
# i=0
# # a = path[i]
# # b = path[i + 10]
# # c = path[i + 20]
# # curvature = calculate_curvature(a, b, c)
# i=i+1
# print(i)
# return curvature
class VehicleState:
# state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0)
def __init__(self, x=88, y=47, yaw=yaw, v=0.0):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a, delta):
state.x =state.x + state.v * math.cos(state.yaw) * dt
# print("state.x",state.x)
state.y = state.y + state.v * math.sin(state.yaw) * dt
# print("state.y",state.y)
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
# print('state.yaw',state.yaw)
state.v = state.v + a * dt
return state
def PControl(target, current):
a = Kp * (target - current)
return a
def remove_adjacent_duplicates(coords):
result = []
for coord in coords:
if not result or coord != result[-1]:
result.append(coord)
print('coor',result)
def pure_pursuit_control(state, cx, cy, pind):
global ag,f, Lfc, delta, curvature, A, B, C
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
# print('tx1',tx)
ty = cy[ind]
# print('ty1',ty)
TX.append((tx, ty))
# print(TX)
else:
tx = cx[-1]
print('tx2',tx)
ty = cy[-1]
print('ty2',ty)
ind = len(cx) - 1
T.append(TX)
# print('T0',T[0][0])
# print('t',TX)
# A=0
# if len(T)>20:
# remove_adjacent_duplicates(T)
# seen = set()
# coords_without_duplicates = []
# for coord in T:
# if coord not in seen:
# coords_without_duplicates.append(coord)
# seen.add(coord)
# else:
# coords_without_duplicates.remove(coord)
#
# print(coords_without_duplicates)
# return result
TX_TY = [x for i, x in enumerate(T[0]) if i == 0 or x != T[0][i - 1]]
# print('txty',TX_TY)
if len(TX_TY)>16:
A=TX_TY[-1]
B=TX_TY[-4]
C=TX_TY[-8]
calculate_curvature(A,B,C)
if curvature>0:
a.append(curvature)
print('a', a)
# if len(T)>300:
# for i in range(len(T)):
# global curvature
# curvature=calculate_curvature(T[0][i],T[0][i+10],T[0][i+20])
# print('cur,1',curvature)
# print('to',T[0][i])
# print('t1',T[0][i+10])
# print('t2',T[0][i+20])
if curvature>=0.18:
Lfc=0.05
# print('cur', curvature)
# print('lfc', Lfc)
if 0.1<=curvature< 0.18:
Lfc = 0.2
# print('cur', curvature)
# print('lfc', Lfc)
else:
Lfc = 1.5
# print('cur', curvature)
# print('lfc', Lfc)
# print('A',A)
alpta_1=math.atan2(ty - state.y, tx - state.x)
alpha = alpta_1- state.yaw
# print('alpha',alpha)
delta = math.atan2(2.0 * L * math.sin(alpha) / Lfc, 1.0)
# print('delta',delta)
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)
# print('deltal1',delta1)
angle = delta * 57.3
#
# fangxp = int((angle) *18)
# # fxp=int(fangxp)
# d=(fangxp-f)
# print('d',d)
# if d>4:
# d=4
# fangxp = f + d
# fangxp=f+d
#
# if d<-4:
# d=-4
# fangxp=f+d
# f = fangxp
# print('f', f)
#
#
# # print('fangx[',fangxp)
#
#
# ag.append(fangxp)
# print('ag',ag)
#
# print('fxp',fangxp)
time.sleep(0.04)
# print(delta)
return delta, ind
def calc_target_index(state, cx, cy):
# 搜索最临近的路点
dx = [state.x - icx for icx in cx]
# print('dx',dx)
dy = [state.y - icy for icy in cy]
# print('dy',dy)
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
# print('d',d)
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
return ind
def main():
global x1,y1
# 设置目标路点
cx = [
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 = [
-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 = 2 / 3.6 # [m/s]
T = 500.0 # 最大模拟时间
# 设置车辆的出事状态
yaw=math.radians(450-(347+180-360))
x_car = np.cos(yaw) * 2.3 + float(100)
# print('x',x_car)
# north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
y_car = -np.sin(yaw) * 2.3 + float(-9.78)
# print('yaw_1',yaw)
state = VehicleState(x =x_car, y=y_car, yaw = float(yaw), v = 0)
# 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 lastIndex > target_ind:
ai = PControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
fxp=int(di*57.3*18)
# print('fxp1',fxp)
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=yaw[-1]
# print('yaw',yaw[-1])
v.append(state.v)
# print('v',v[-1])
t.append(time)
# logger.add('file.log')
# logger.debug('角度 {},y {},x {}',str(yaw1),str(y1),str(x1))
# f = open('data1,txt', 'a')
# # print('fff')
# f.write("角度"+str(yaw1))
# f.write('\n')
# f.write("\n")
# f.close()
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)
plt.ioff()
plt.show()
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=main)
t8=threading.Thread(target=CUR.calculate_curvature)
# t6 = threading.Thread(target=tcp4.zuo_biao)
# t7=threading.Thread(target=txt2.txt)
#
# # 启动线程
# t7.start()
t5.start()
# t6.start()
# t1.start()
# t2.start()
# t3.start()
# t4.start()

547
pure_change_all.py
Unescape Escape View File

@ -0,0 +1,547 @@
"""
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()

422
pure_test.py
Unescape Escape View File

@ -0,0 +1,422 @@
import time
# import txt2
import numpy as np
import math
import matplotlib.pyplot as plt
from loguru import logger
# import tcp4
import sqlite3
import threading
import CUR
ag = [0,0]
a = []
# import test5
k = 0 # 前视距离系数
Lfc =2# 前视距离
Kp = 1.0 # 速度P控制器系数
dt = 0.1 # 时间间隔，单位：s
L = 2.7 # 车辆轴距，单位：m
x1=[]
y1=[]
# yaw1 = math.radians(414)
# print('y',yaw1)
# arc=math.atan2(1, 1)
# print('acr',arc)
yaw = math.radians(450 - (344 + 180 - 360))
f=0
T = []
TX = []
curvature=0
A=0
B=0
C=0
def calculate_curvature(point_a, point_b, point_c):
global curvature
# 将输入点转换为 NumPy 数组
a = np.array(point_a)
b = np.array(point_b)
c = np.array(point_c)
# 计算各边长
ab = np.linalg.norm(a - b)
bc = np.linalg.norm(b - c)
ca = np.linalg.norm(c - a)
# 使用海伦公式计算三角形面积
s = (ab + bc + ca) / 2
area = np.sqrt(s * (s - ab) * (s - bc) * (s - ca))
# 计算曲率
curvature = 4 * area / (ab * bc * ca)
print('cur',curvature)
# i=0
# # a = path[i]
# # b = path[i + 10]
# # c = path[i + 20]
# # curvature = calculate_curvature(a, b, c)
# i=i+1
# print(i)
# return curvature
class VehicleState:
# state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0)
def __init__(self, x=88, y=47, yaw=yaw, v=0.0):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a, delta):
state.x =state.x + state.v * math.cos(state.yaw) * dt
# print("state.x",state.x)
state.y = state.y + state.v * math.sin(state.yaw) * dt
# print("state.y",state.y)
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
# print('state.yaw',state.yaw)
state.v = state.v + a * dt
return state
def PControl(target, current):
a = Kp * (target - current)
return a
def remove_adjacent_duplicates(coords):
result = []
for coord in coords:
if not result or coord != result[-1]:
result.append(coord)
print('coor',result)
def pure_pursuit_control(state, cx, cy, pind):
global ag,f, Lfc, delta, curvature, A, B, C
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
# print('tx1',tx)
ty = cy[ind]
# print('ty1',ty)
TX.append((tx, ty))
# print(TX)
else:
tx = cx[-1]
print('tx2',tx)
ty = cy[-1]
print('ty2',ty)
ind = len(cx) - 1
T.append(TX)
# print('T0',T[0][0])
# print('t',TX)
# A=0
# if len(T)>20:
# remove_adjacent_duplicates(T)
# seen = set()
# coords_without_duplicates = []
# for coord in T:
# if coord not in seen:
# coords_without_duplicates.append(coord)
# seen.add(coord)
# else:
# coords_without_duplicates.remove(coord)
#
# print(coords_without_duplicates)
# return result
TX_TY = [x for i, x in enumerate(T[0]) if i == 0 or x != T[0][i - 1]]
# print('txty',TX_TY)
if len(TX_TY)>15:
A=TX_TY[-1]
B=TX_TY[-15]
C=TX_TY[-7]
calculate_curvature(A,B,C)
if curvature>0:
a.append(curvature)
print('a', a)
# if len(T)>300:
# for i in range(len(T)):
# global curvature
# curvature=calculate_curvature(T[0][i],T[0][i+10],T[0][i+20])
# print('cur,1',curvature)
# print('to',T[0][i])
# print('t1',T[0][i+10])
# print('t2',T[0][i+20])
# if curvature>=0.18:
# Lfc=0.05
print('cur', curvature)
# # print('lfc', Lfc)
# if 0.1<=curvature< 0.15:
# Lfc = 0.3
# # print('cur', curvature)
# # print('lfc', Lfc)
# if 0.15 <= curvature < 0.2:
# Lfc = 0.05
# else:
# Lfc =0.8
# print('cur', curvature)
# print('lfc', Lfc)
#
if len(TX_TY)>20:
Lfc=Lfc*curvature*0.5
# Lfc=round(Lfc)
if curvature<0.07:
Lfc=0.4558
if Lfc==0:
Lfc=1
if np.isnan(Lfc):
Lfc = 1
print('a is NaN')
print('lcf',Lfc)
alpta_1=math.atan2(ty - state.y, tx - state.x)
alpha = alpta_1- state.yaw
# print('alpha',alpha)
delta = math.atan2(2.0 * L * math.sin(alpha) / Lfc, 1.0)
# print('delta',delta)
#
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)
# print('deltal1',delta1)
angle = delta * 57.3
#
fangxp = int((angle) *18)
fxp=int(fangxp)
# d=(fangxp-f)
# print('d',d)
# if d>4:
# d=4
# fangxp = f + d
# fangxp=f+d
#
# if d<-4:
# d=-4
# fangxp=f+d
# f = fangxp
# print('f', f)
#
#
# # print('fangx[',fangxp)
#
#
# ag.append(fangxp)
# print('ag',ag)
#
print('fxp',fangxp)
time.sleep(0.04)
# print(delta)
return delta, ind
def calc_target_index(state, cx, cy):
# 搜索最临近的路点
dx = [state.x - icx for icx in cx]
# print('dx',dx)
dy = [state.y - icy for icy in cy]
# print('dy',dy)
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
# print('d',d)
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
return ind
def main():
global x1,y1
# 设置目标路点
cx = [
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 = [
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 = 3/ 3.6 # [m/s]
T = 500.0 # 最大模拟时间
# 设置车辆的出事状态
yaw=math.radians(450-(347+180-360))
x_car = np.cos(yaw) * 2.3 + float(96.048)
# print('x',x_car)
# north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
y_car = -np.sin(yaw) * 2.3 + float( 9.935)
# print('yaw_1',yaw)
state = VehicleState(x =96, y=10, yaw = float(yaw), v = 0)
# 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 lastIndex > target_ind:
ai = PControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
# fxp=int(di*57.3*18)
# print('fxp1',fxp)
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=yaw[-1]
# print('yaw',yaw[-1])
v.append(state.v)
# print('v',v[-1])
t.append(time)
# logger.add('file.log')
# logger.debug('角度 {},y {},x {}',str(yaw1),str(y1),str(x1))
# f = open('data1,txt', 'a')
# # print('fff')
# f.write("角度"+str(yaw1))
# f.write('\n')
# f.write("\n")
# f.close()
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)
plt.ioff()
plt.show()
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=main)
t8=threading.Thread(target=CUR.calculate_curvature)
# t6 = threading.Thread(target=tcp4.zuo_biao)
# t7=threading.Thread(target=txt2.txt)
#
# # 启动线程
# t7.start()
t5.start()
# t6.start()
# t1.start()
# t2.start()
# t3.start()
# t4.start()

393
pure_zhixian.py
Unescape Escape View File

@ -0,0 +1,393 @@
"""
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()

128
tcp4.py
Unescape Escape View File

@ -0,0 +1,128 @@
import datetime
import math
import socket
import numpy as np
from loguru import logger
clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
clientSocket.connect(('192.168.43.100', 4002))
# # 发
# sendData = input('>>')
# clientSocket.send(sendData.encode('gbk'))
# 收
# jishi=0
# while jishi<= 30:
x = 0
y = 0
yaw = 0
x_car = 0
y_car = 0
heading = 0
east = 0
north = 0
beidou = 0
def zuo_biao():
global yaw, x_car, y_car, east, north, beidou, heading
while True:
recvData = clientSocket.recv(1024)
tcpdata = recvData.decode('gbk')
# print(tcpdata)
beidou = tcpdata.split(',')
# print(beidou)
# print(len(beidou))
# KSXT = beidou[0]
# if KSXT=='$KSXT':
KSXT = beidou[0] # 帧头
time0 = beidou[1] # 时间
lon = beidou[2] # 经度
lat = beidou[3] # 纬度
height = beidou[4] # 海拔
heading = beidou[5] # 方位角
heading = float(heading)
heading = heading + 180
if heading > 360:
heading = heading - 360
if 0 < heading <= 90:
yaw = math.radians(90 - float(heading))
yaw = round(yaw, 3)
else:
yaw = math.radians(450 - float(heading))
yaw = round(yaw, 3)
print("yaw", yaw)
# pitch = beidou[6] # 俯仰角
# tracktrue = beidou[7] # 速度角
# vel = beidou[8] # 水平速度
# roll = beidou[9] # 横滚 空
# pos_qual = beidou[10] # GNSS定位质量指示符0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
# heading_qual = beidou[11] # GNSS定向质量指示符 0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
# solnsvsh = beidou[12] # 前天线使用卫星数 前天线当前参与解算的卫星数量
# solnsvsb = beidou[13] # 后天线使用卫星数 前天线当前参与解算的卫星数量
if abs(x_car-float(east))>10 or abs(y_car-float(north))>10:
x_car=float(east)
y_car=float(north)
east = beidou[14] # 东向位置坐标：以基站为原点的地理坐标系下的东向位置，单位：米，小数点后 3 位
x_car = np.cos(yaw) * 2.3 + float(east)
x_car = round(x_car, 3)
# print('x',x_car)
north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
y_car = -np.sin(yaw) * 2.3 + float(north)
y_car = round(y_car, 3)
shijian = datetime.datetime.now()
# logger.add(
# 'x,y记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
# logger.debug('北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(east), str(north), str(x_car),
# str(y_car), str(beidou[5]), str(heading))
# up = beidou[16] # 天向位置坐标：以基站为原点的地理坐标系下的天顶向位置，单位：米，小数点后 3 位
# eastvel = beidou[17] # 东向速度：地理坐标系下的东向速度，小数点后 3位，单位：Km/h(如无为空)
# northvel = beidou[18] # 北向速度：地理坐标系下的北向速度，小数点后 3位，单位：Km/h(如无为空)
# uphvel = beidou[19] # 天向速度：地理坐标系下的天顶向速度，小数点后3 位，单位：Km/h(如无为空)
# unknown = beidou[20]
# check = beidou[21] # 异或校验（十六进制字符串，从帧头开始校验）
# east = float(east)
# north = float(north)
# print('接收时间为:',datetime.datetime.now())
# print('方位角：%s; 东向位置坐标：%s; 北向位置坐标：%s; ' % ( heading, east, north))
# print('\n')
# shijian=datetime.datetime.now()
# file = open('1.txt', 'a')
# file.write(str('方位角：%s; 东向位置坐标：%s; 北向位置坐标：%s; ' % (heading, east, north)))
# file.write('\n')
# jishi=jishi+1
# time.sleep(0.05)
# else:
# print('未接收到定位消息，重新连接')
# clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# clientSocket.connect(('192.168.43.100', 4002))
# # clientSocket.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
# if 0.2>=d >= 0.1:
# print('大于', d)
# print('\n')
# file = open('1.txt', 'a')
# file.write(str('方位角：%s; 东向位置坐标：%s; 北向位置坐标：%s;与上一点的距离为%s ' % (heading, x, y,d)))
# file.write('\n')
# x = east
# y = north
# print(x, y)
# else:
# print('不在范围', d)
# print('\n')
# if x==0 and y==0:
# x = east
# y = north
# time.sleep(0.05)
# zuo_biao()
Write Preview
Loading…
Cancel
Save