V0.2 倒车所有环节（直线、右倒库、出库、左倒库），添加倒车纯跟踪

gear_change.py

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

geartest2.py

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

pure_change_all.py

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

pure_zhixian.py

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