diff --git a/pure_last.py b/pure_last.py
new file mode 100644
index 0000000..a8b3a0b
--- /dev/null
+++ b/pure_last.py
@@ -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()
\ No newline at end of file
diff --git a/pure_test.py b/pure_test.py
new file mode 100644
index 0000000..8ddcf0c
--- /dev/null
+++ b/pure_test.py
@@ -0,0 +1,382 @@
+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)
+    if  len(TX_TY)>20:
+        Lfc=Lfc*curvature
+    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 = [
+           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()
\ No newline at end of file