.idea/.gitignore

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

.idea/inspectionProfiles/Project_Default.xml

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

.idea/inspectionProfiles/profiles_settings.xml

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

.idea/misc.xml

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

.idea/modules.xml

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

.idea/pure_自适应.iml

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

.idea/toolchains.xml

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

.idea/vcs.xml

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

1132.py

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

MPC.py

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

MPc2.py

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

curvature.py

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

gear_change.py

@@ -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()

geartest2.py

@@ -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()

pure_CUR_last.py

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

pure_change_all.py

@@ -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()

pure_test.py

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

pure_zhixian.py

@@ -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()

tcp4.py

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