From 58550159c166c54535060c7a60dcf79da6de097f Mon Sep 17 00:00:00 2001 From: wrc_back_GXK <2435502783@qq.com> Date: Tue, 9 May 2023 20:35:01 +0800 Subject: [PATCH] v0.1 --- pure5.1.py | 212 ++++++++++++++++++++ pure_back_1.py | 512 +++++++++++++++++++++++++++++++++++++++++++++++++ sped.py | 41 ++++ tcp4.py | 130 +++++++++++++ test5.py | 398 ++++++++++++++++++++++++++++++++++++++ test_angle.py | 14 ++ turndb.db | Bin 0 -> 8192 bytes turndb.py | 123 ++++++++++++ 8 files changed, 1430 insertions(+) create mode 100644 pure5.1.py create mode 100644 pure_back_1.py create mode 100644 sped.py create mode 100644 tcp4.py create mode 100644 test5.py create mode 100644 test_angle.py create mode 100644 turndb.db create mode 100644 turndb.py diff --git a/pure5.1.py b/pure5.1.py new file mode 100644 index 0000000..56410dd --- /dev/null +++ b/pure5.1.py @@ -0,0 +1,212 @@ +import datetime +import time +from loguru import logger +import numpy as np +import math +# import matplotlib.pyplot as plt +import tcp4 +import sqlite3 +import threading +import test5 +import sped + +k = 0 # 前视距离系数 +Lfc = 6# 前视距离 +Kp = 1.0 # 速度P控制器系数 +dt = 0.1 # 时间间隔，单位：s +L = 2.7 # 车辆轴距，单位：m +tx=0 +ty=0 +fangxp=0 + +class VehicleState: + # state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0) + + def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed): + self.x = x + self.y = y + self.yaw = yaw + self.v = v + + +def update(state, a, delta): + state.x = tcp4.x_car + sped.speed * math.cos(tcp4.yaw) * dt + # print("state",state.x) + state.y = tcp4.y_car + sped.speed * math.sin(tcp4.yaw) * dt + state.yaw = tcp4.yaw + sped.speed / L * math.tan(delta) * dt + state.v = sped.speed + a * dt + return state + + +def PControl(target, current): + a = Kp * (target - current) + return a + + +def pure_pursuit_control(state, cx, cy, pind): + global tx,ty,fangxp + ind = calc_target_index(state, cx, cy) + # if pind >= ind: + # ind = pind + if ind < len(cx): + tx = cx[ind] + print('tx', tx) + ty = cy[ind] + print('ty', ty) + else: + tx = cx[-1] + # print('tx1',tx) + + ty = cy[-1] + + # print('ty1',ty) + + ind = len(cx) - 1 + + alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw + # print('alpha',alpha) + if state.v < 0: # back + alpha = math.pi - alpha + Lf = k * state.v + Lfc + delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) + if delta > np.pi / 6.0: + delta = np.pi / 6.0 + # print('delta1', delta) + + elif delta < - np.pi / 6.0: + delta = - np.pi / 6.0 + # print('delta2', delta) + angle = delta * 57.3 + fangxp = -int((angle) * 18) + print('fxp', fangxp) + + file = open('1.txt', 'a') + file.write(str('北向位置坐标：%s; ' % (fangxp))) + file.write('\n') + + db_file = "turndb.db" + conn = sqlite3.connect(db_file) + cur = conn.cursor() + sql='INSERT INTO turndb(id) VALUES(1);' + sql = "update turndb set angle='%s' WHERE id = 1" % fangxp + cur.execute(sql) + conn.commit() + cur.close() + conn.close() + time.sleep(0.04) + # print(delta) + return delta, ind + + +def calc_target_index(state, cx, cy): + # 搜索最临近的路点 + dx = [state.x - icx for icx in cx] + dy = [state.y - icy for icy in cy] + d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] + + ind = d.index(min(d)) + # print('ind',ind) + L = 0.0 + Lf = k * state.v + Lfc + while Lf > L and (ind + 1) < len(cx): + dx = cx[ind + 1] - cx[ind] + dy = cx[ind + 1] - cx[ind] + L += math.sqrt(dx ** 2 + dy ** 2) + ind += 1 + # print('ind1', ind) + + return ind + + +def main(): + # 设置目标路点 + cx = [75.527, 75.629, 75.727, 75.826, 75.924, 76.026, 76.136, 76.235, 76.338, 76.438, 76.548, 76.652, 76.771, + 76.891, 76.996, 77.108, 77.227, 77.339, 77.442, 77.551, 77.655, 77.761, 77.872, 77.981, 78.087, 78.197, 78.32, + 78.428, 78.539, 78.647, 78.753, 78.849, 78.968, 79.078, 79.186, 79.285, 79.387, 79.487, 79.592, 79.694, + 79.795, 79.892, 79.988, 80.092, 80.189, 80.272, 80.361, 80.447, 80.53, 80.611, 80.695, 80.785, 80.871, 80.957, + 81.041, 81.117, 81.197, 81.267, 81.345, 81.42, 81.489, 81.554, 81.605, 81.648, 81.685, 81.709, 81.738, 81.772, + 81.802, 81.854, 81.907, 81.966, 82.025, 82.081, 82.138, 82.195, 82.248, 82.299, 82.355, 82.408, 82.46, 82.51, + 82.558, 82.609, 82.657, 82.702, 82.749, 82.793, 82.83, 82.865, 82.896, 82.938, 82.971, 83.001, 83.03, 83.057, + 83.084, 83.111, 83.127, 83.136, 83.128, 83.111, 83.086, 83.061, 83.034, 82.998, 82.958, 82.918, 82.886, 82.853, + 82.819, 82.789, 82.755, 82.71, 82.678, 82.647, 82.614, 82.576, 82.548, 82.521, 82.503] + cy = [-20.969, -20.977, -21.0, -21.023, -21.046, -21.062, -21.08, -21.097, -21.108, -21.122, -21.136, -21.148, + -21.151, -21.155, -21.162, -21.159, -21.159, -21.158, -21.153, -21.145, -21.134, -21.131, -21.115, -21.101, + -21.083, -21.066, -21.054, -21.032, -21.001, -20.973, -20.942, -20.907, -20.873, -20.837, -20.8, -20.755, + -20.717, -20.675, -20.626, -20.578, -20.528, -20.48, -20.428, -20.371, -20.311, -20.247, -20.192, -20.132, + -20.073, -20.013, -19.952, -19.886, -19.812, -19.74, -19.671, -19.597, -19.522, -19.45, -19.369, -19.293, + -19.217, -19.127, -19.034, -18.94, -18.836, -18.737, -18.632, -18.529, -18.428, -18.329, -18.24, -18.155, + -18.066, -17.978, -17.886, -17.799, -17.707, -17.619, -17.524, -17.435, -17.342, -17.254, -17.161, -17.061, + -16.965, -16.865, -16.76, -16.656, -16.561, -16.462, -16.365, -16.25, -16.142, -16.036, -15.923, -15.824, + -15.723, -15.612, -15.507, -15.398, -15.297, -15.189, -15.076, -14.977, -14.871, -14.763, -14.656, -14.556, + -14.46, -14.36, -14.26, -14.161, -14.051, -13.941, -13.841, -13.741, -13.639, -13.531, -13.433, -13.328, -13.228] + target_speed = 5 / 3.6 # [m/s] + T = 500.0 # 最大模拟时间 + # 设置车辆的出事状态 + state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed) + # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0) + lastIndex = len(cx) - 1 + time = 0.0 + x = [state.x] + # print('x',x) + y = [state.y] + # print('y',y) + yaw = [state.yaw] + v = [state.v] + t = [0.0] + target_ind = calc_target_index(state, cx, cy) + while True: + # print('heading',tcp4.heading) + ai = PControl(target_speed, state.v) + di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) + state = update(state, ai, di) + time = time + dt + x.append(state.x) + x1=round(x[-1],3) + print('x1', x[-1]) + y.append(state.y) + y1=round(y[-1],3) + print('y1', y[-1]) + yaw.append(state.yaw) + yaw1=y[-1]*57.3 + print('yaw', yaw[-1]) + t.append(time) + # logger.add('file.log') + # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north)) + v.append(state.v) + # print('v', v[-1]) + + t.append(time) + # plt.cla() + # plt.plot(cx, cy, ".r", label="course") + # plt.plot(x, y, "-b", label="trajectory") + # plt.plot(cx[target_ind], cy[target_ind], "go", label="target") + # plt.axis("equal") + # plt.grid(True) + # plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4]) + # plt.pause(0.001) + 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{},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 __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) + t6 = threading.Thread(target=tcp4.zuo_biao) + t7 = threading.Thread(target=sped.sped) + # 启动线程 + t5.start() + t6.start() + t7.start() + t1.start() + t2.start() + t3.start() + # t4.start() diff --git a/pure_back_1.py b/pure_back_1.py new file mode 100644 index 0000000..4e24024 --- /dev/null +++ b/pure_back_1.py @@ -0,0 +1,512 @@ +import time +from loguru import logger +import numpy as np +import math +# import matplotlib.pyplot as plt +import tcp4 +import sqlite3 +import threading +import test5 +import sped + +k = 0 # 前视距离系数 +Lfc = 5 # 前视距离 +Kp = 1.0 # 速度P控制器系数 +dt = 0.1 # 时间间隔，单位：s +L = 2.7 # 车辆轴距，单位：m +fangxp = 0 + + +class VehicleState: + # state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0) + + def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed): + self.x = x + self.y = y + self.yaw = yaw + self.v = v + + +def update(state, a, delta): + state.x = tcp4.x_car + sped.speed * math.cos(tcp4.car_yaw) * dt + # print("state",state.x) + state.y = tcp4.y_car + sped.speed * math.sin(tcp4.car_yaw) * dt + state.yaw = tcp4.car_yaw + sped.speed / L * math.tan(delta) * dt + state.v = sped.speed + a * dt + return state + + +def PControl(target, current): + a = Kp * (target - current) + return a + + +def pure_pursuit_control(state, cx, cy, pind): + global fangxp + ind = calc_target_index(state, cx, cy) + # if pind >= ind: + # ind = pind + if ind < len(cx): + tx = cx[ind] + print('tx', tx) + ty = cy[ind] + print('ty', ty) + else: + tx = cx[-1] + # print('tx1',tx) + + ty = cy[-1] + + # print('ty1',ty) + + ind = len(cx) - 1 + + alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw + # print('alpha',alpha) + if state.v < 0: # back + alpha = math.pi - alpha + Lf = k * state.v + Lfc + delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) + if delta > np.pi / 6.0: + delta = np.pi / 6.0 + # print('delta1', delta) + + elif delta < - np.pi / 6.0: + delta = - np.pi / 6.0 + # print('delta2', delta) + angle = delta * 57.3 + fangxp = int(angle * 18) + print('fxp', fangxp) + + db_file = "turndb.db" + conn = sqlite3.connect(db_file) + cur = conn.cursor() + # sql='INSERT INTO turndb(id) VALUES(1);' + sql = "update turndb set angle='%s' WHERE id = 1" % fangxp + cur.execute(sql) + conn.commit() + cur.close() + conn.close() + time.sleep(0.04) + # print(delta) + return delta, ind + + +def calc_target_index(state, cx, cy): + # 搜索最临近的路点 + dx = [state.x - icx for icx in cx] + dy = [state.y - icy for icy in cy] + d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] + + ind = d.index(min(d)) + # print('ind',ind) + L = 0.0 + Lf = k * state.v + Lfc + while Lf > L and (ind + 1) < len(cx): + dx = cx[ind + 1] - cx[ind] + dy = cx[ind + 1] - cx[ind] + L += math.sqrt(dx ** 2 + dy ** 2) + ind += 1 + # print('ind1', ind) + + return ind + + +def main(): + # 设置目标路点 + cx = [84.61, 84.639, 84.667, 84.7, 84.722, 84.743, 84.775, 84.806, 84.837, 84.869, 84.9, 84.928, 84.949, 84.972, + 85.004, 85.028, 85.06, 85.096, 85.123, 85.149, 85.177, 85.208, 85.24, 85.269, 85.298, 85.331, 85.361, 85.391, + 85.423, + 85.457, 85.488, 85.526, 85.547, 85.572, 85.602, 85.622, 85.648, 85.673, 85.694, 85.721, 85.741, 85.762, + 85.786, 85.81, 85.832, 85.856, 85.88, 85.899, 85.923, 85.948, 85.973, 85.996, 86.017, 86.042, 86.068, 86.089, + 86.113, + 86.131, + 86.154, 86.175, 86.195, 86.217, 86.245, 86.262, 86.288, 86.307, 86.33, 86.35, 86.376, 86.398, 86.423, + 86.448, 86.474, 86.499, 86.52, 86.548, 86.575, 86.602, 86.624, 86.65, 86.676, 86.7, 86.72, 86.743, 86.77, + 86.793, 86.821, 86.836, 86.861, 86.887, 86.916, 86.943, 86.964, 86.988, 87.01, 87.039, 87.066, 87.092, 87.12, + 87.145, + 87.169, 87.192, 87.221, 87.243, 87.271, 87.295, 87.324, 87.348, 87.369, 87.391, 87.417, 87.445, 87.475, + 87.501, 87.525, 87.546, 87.575, 87.6, 87.629, 87.656, 87.685, 87.712, 87.736, 87.77, 87.794, 87.823, 87.852, + 87.885, + 87.916, 87.939, 87.963, 87.994, 88.019, 88.053, 88.084, 88.114, 88.138, 88.17, 88.196, 88.228, 88.257, 88.288, + 88.318, 88.343, 88.373, 88.4, 88.427, 88.461, 88.489, 88.518, 88.548, 88.58, 88.606, 88.633, 88.66, 88.693, + 88.712, 88.738, 88.765, 88.791, 88.826, 88.861, 88.888, 88.914, 88.943, 88.969, 88.996, 89.022, 89.052, + 89.082, + 89.113, 89.135, 89.158, 89.184, 89.218, 89.248, 89.272, 89.297, 89.334, 89.362, 89.39, 89.412, 89.445, 89.475, + 89.504, + 89.529, 89.561, 89.589, 89.617, 89.65, 89.673, 89.713, 89.736, 89.764, 89.792, 89.825, 89.853, 89.88, 89.913, + 89.939, 89.968, 89.997, 90.026, 90.057, 90.086, 90.119, 90.144, 90.165, 90.194, 90.226, 90.251, 90.279, + 90.306, 90.336, 90.365, 90.393, 90.424, 90.447, 90.477, 90.508, 90.533, 90.568, 90.601, 90.627, 90.654, + 90.685, + 90.713, 90.745, 90.776, 90.806, 90.835, 90.862, 90.894, 90.922, 90.949, 90.982, 91.009, 91.043, 91.069, + 91.102, + 91.138, 91.168, 91.199, 91.228, 91.26, 91.288, 91.316, 91.345, 91.372, 91.407, 91.436, 91.464, 91.494, 91.522, + 91.552, + 91.578, 91.603, 91.629, 91.66, 91.69, 91.713, 91.742, 91.774, 91.803, 91.823, 91.851, 91.877, 91.908, 91.935, + 91.963, 91.993, 92.019, 92.045, 92.071, 92.096, 92.127, 92.154, 92.184, 92.207, 92.233, 92.264, 92.297, + 92.324, 92.347, 92.374, 92.403, 92.43, 92.459, 92.487, 92.513, 92.544, 92.568, 92.596, 92.623, 92.652, 92.681, + 92.706, + 92.732, 92.762, 92.79, 92.826, 92.856, 92.881, 92.91, 92.936, 92.967, 92.991, 93.023, 93.047, 93.075, 93.105, + 93.127, 93.155, 93.184, 93.214, 93.24, 93.263, 93.29, 93.32, 93.348, 93.376, 93.407, 93.433, 93.465, 93.498, + 93.524, 93.559, 93.588, 93.621, 93.647, 93.675, 93.703, 93.735, 93.763, 93.797, 93.83, 93.858, 93.888, 93.914, + 93.944, 93.975, 94.001, 94.026, 94.052, 94.083, 94.11, 94.141, 94.164, 94.188, 94.213, 94.24, 94.27, 94.296, + 94.328, 94.355, 94.389, 94.418, 94.451, 94.48, 94.509, 94.535, 94.577, 94.607, 94.639, 94.672, 94.702, 94.724, + 94.756, 94.786, 94.816, 94.847, 94.879, 94.907, 94.941, 94.97, 94.999, 95.028, 95.055, 95.089, 95.12, 95.149, + 95.177, 95.215, 95.244, 95.275, 95.29, 95.32, 95.351, 95.379, 95.406, 95.435, 95.466, 95.493, 95.522, 95.546, + 95.576, 95.612, 95.642, 95.674, 95.706, 95.732, 95.782, 95.812, 95.844, 95.872, 95.9, 95.925, 95.95, 95.985, + 96.014, 96.048, 96.078, 96.109, 96.139, 96.17, 96.203, 96.234, 96.266, 96.299, 96.33, 96.364, 96.395, 96.424, + 96.456, 96.487, 96.514, 96.546, 96.576, 96.609, 96.638, 96.667, 96.701, 96.735, 96.765, 96.795, 96.825, + 96.855, 96.888, 96.918, 96.953, 96.982, 97.013, 97.045, 97.095, 97.123, 97.157, 97.179, 97.208, 97.243, + 97.277, + 97.308, 97.342, 97.376, 97.407, 97.435, 97.47, 97.501, 97.534, 97.565, 97.595, 97.624, 97.649, 97.684, 97.716, + 97.749, + 97.776, 97.811, 97.84, 97.869, 97.901, 97.931, 97.96, 97.984, 98.016, 98.042, 98.07, 98.1, 98.128, 98.155, + 98.188, 98.218, 98.244, 98.273, 98.304, 98.339, 98.366, 98.397, 98.42, 98.465, 98.495, 98.523, 98.549, 98.578, + 98.61, + 98.637, 98.667, 98.7, 98.729, 98.757, 98.789, 98.819, 98.843, 98.872, 98.892, 98.921, 98.955, 98.984, 99.01, + 99.04, 99.06, 99.09, 99.121, 99.152, 99.178, 99.208, 99.233, 99.26, 99.29, 99.32, 99.349, 99.376, 99.4, + 99.428, + 99.456, 99.486, 99.514, 99.544, 99.571, 99.598, 99.627, 99.654, 99.682, 99.714, 99.741, 99.769, 99.799, + 99.831, + 99.859, 99.888, 99.932, 99.957, 99.987, 100.012, 100.043, 100.076, 100.108, 100.135, 100.167, 100.191, + 100.221, + 100.252, 100.281, 100.311, 100.342, 100.371, 100.394, 100.427, 100.455, 100.49, 100.522, 100.553, 100.58, + 100.614, + 100.641, 100.672, 100.702, 100.728, 100.761, 100.792, 100.824, 100.849, 100.876, 100.909, 100.942, 100.967, + 100.999, + 101.031, 101.059, 101.09, 101.121, 101.15, 101.175, 101.204, 101.235, 101.264, 101.288, 101.32, 101.348, + 101.38, 101.404, 101.433, 101.468, 101.495, 101.521, 101.551, 101.575, 101.604, 101.632, 101.66, 101.683, + 101.716, + 101.744, 101.776, 101.808, 101.83, 101.854, 101.889, 101.918, 101.945, 101.967, 101.99, 102.013, 102.033, + 102.057, + 102.082, 102.102, 102.116, 102.134, 102.149, 102.164, 102.176, 102.184, 102.187, 102.185, 102.183, 102.176, + 102.165, + 102.141, 102.112, 102.086, 102.058, 102.031, 101.989, 101.955, 101.912, 101.861, 101.805, 101.748, 101.68, + 101.606, 101.522, 101.44, 101.353, 101.272, 101.185, 101.09, 100.999, 100.9, 100.804, 100.706, 100.611, + 100.505, + 100.406, 100.299, 100.195, 100.085, 99.979, 99.867, 99.739, 99.62, 99.493, 99.363, 99.231, 99.102, 98.965, + 98.83, + 98.623, 98.48, 98.339, 98.198, 98.06, 97.927, 97.794, 97.659, 97.534, 97.402, 97.268, 97.143, 97.023, 96.896, + 96.773, + 96.645, 96.518, 96.393, 96.26, 96.127, 95.988, 95.845, 95.706, 95.558, 95.409, 95.258, 95.102, 94.945, 94.788, + 94.633, 94.475, 94.315, 94.16, 94.002, 93.903, 93.75, 93.589, 93.434, 93.274, 93.118, 92.963, 92.807, 92.648, + 92.487, 92.333, 92.18, 92.022, 91.864, 91.711, 91.553, 91.407, 91.252, 91.102, 90.954, 90.806, 90.652, 90.511, + 90.354, 90.203, 90.049, 89.89, 89.737, 89.581, 89.422, 89.26, 89.108, 88.949, 88.793, 88.636, 88.48, 88.326, + 88.168, 88.01, 87.855, 87.7, 87.548, 87.395, 87.243, 87.088, 86.939, 86.785, 86.635, 86.482, 86.334, 86.185, + 86.038, 85.886, 85.666, 85.516, 85.367, 85.217, 85.067, 84.913, 84.766, 84.62, 84.473, 84.322, 84.171, 84.022, + 83.871, 83.722, 83.576, 83.429, 83.282, 83.132, 82.981, 82.834, 82.687, 82.524, 82.375, 82.225, 82.072, + 81.918, 81.772, 81.618, 81.457, 81.309, 81.154, 80.998, 80.843, 80.694, 80.543, 80.385, 80.237, 80.086, + 79.937, + 79.785, 79.639, 79.493, 79.355, 79.205, 79.063, 78.922, 78.784, 78.647, 78.517, 78.384, 78.25, 78.119, 77.986, + 77.856, + 77.741, 77.625, 77.511, 77.403, 77.298, 77.199, 77.065, 76.938, 76.826, 76.707] + cy = [66.014, 65.909, 65.805, 65.693, 65.595, 65.496, 65.384, 65.267, 65.144, 65.021, 64.894, 64.796, 64.695, + 64.595, 64.484, 64.373, 64.26, 64.141, 64.023, 63.9, 63.776, 63.65, 63.521, 63.395, 63.261, 63.128, 62.997, + 62.864, 62.734, 62.595, 62.453, 62.312, 62.214, 62.114, 61.97, 61.87, 61.769, 61.667, 61.565, 61.464, 61.362, + 61.257, 61.157, 61.05, 60.944, 60.842, 60.735, 60.63, 60.523, 60.414, 60.306, 60.198, 60.091, 59.98, 59.867, + 59.755, 59.645, 59.535, 59.419, 59.308, 59.196, 59.088, 58.971, 58.855, 58.736, 58.616, 58.494, 58.373, + 58.253, 58.133, 58.012, 57.892, 57.767, 57.645, 57.525, 57.404, 57.282, 57.163, 57.041, 56.922, 56.801, + 56.682, 56.562, 56.442, 56.323, 56.207, 56.081, 55.967, 55.835, 55.715, 55.585, 55.454, 55.327, 55.196, 55.07, + 54.942, 54.814, 54.69, 54.572, 54.443, 54.317, 54.191, 54.067, 53.949, 53.83, 53.713, 53.603, 53.483, 53.364, + 53.244, 53.126, 53.007, 52.882, 52.76, 52.634, 52.507, 52.386, 52.26, 52.133, 52.002, 51.873, 51.741, 51.607, + 51.472, 51.34, 51.205, 51.072, 50.934, 50.794, 50.655, 50.512, 50.376, 50.235, 50.095, 49.951, 49.808, 49.666, + 49.522, 49.381, 49.237, 49.094, 48.955, 48.814, 48.678, 48.541, 48.403, 48.254, 48.045, 47.903, 47.762, + 47.626, 47.483, 47.337, 47.193, 47.049, 46.903, 46.759, 46.613, 46.465, 46.313, 46.168, 46.027, 45.881, 45.74, + 45.599, 45.457, 45.309, 45.166, 45.021, 44.875, 44.729, 44.582, 44.439, 44.292, 44.143, 43.997, 43.851, + 43.709, 43.568, 43.426, 43.279, 43.136, 42.994, 42.849, 42.704, 42.561, 42.418, 42.273, 42.127, 41.986, + 41.844, 41.7, 41.552, 41.404, 41.257, 41.112, 40.964, 40.818, 40.672, 40.526, 40.381, 40.236, 40.09, 39.941, + 39.797, 39.653, 39.512, 39.362, 39.223, 39.08, 38.94, 38.795, 38.653, 38.508, 38.368, 38.226, 38.078, 37.931, + 37.788, 37.639, 37.496, 37.351, 37.206, 37.059, 36.908, 36.763, 36.613, 36.463, 36.319, 36.172, 36.024, + 35.877, 35.729, 35.58, 35.434, 35.286, 35.139, 34.995, 34.844, 34.695, 34.552, 34.402, 34.256, 34.107, 33.958, + 33.813, 33.663, 33.512, 33.363, 33.211, 33.067, 32.922, 32.773, 32.627, 32.484, 32.336, 32.19, 32.047, 31.903, + 31.76, 31.61, 31.469, 31.325, 31.183, 31.039, 30.901, 30.762, 30.625, 30.488, 30.349, 30.213, 30.074, 29.935, + 29.797, 29.661, 29.522, 29.393, 29.254, 29.102, 28.959, 28.815, 28.671, 28.524, 28.383, 28.241, 28.104, + 27.968, 27.823, 27.69, 27.55, 27.409, 27.267, 27.128, 26.985, 26.842, 26.698, 26.554, 26.406, 26.258, 26.107, + 25.96, 25.808, 25.664, 25.513, 25.366, 25.215, 25.069, 24.927, 24.791, 24.654, 24.513, 24.374, 24.234, 24.098, + 23.954, 23.812, 23.664, 23.521, 23.375, 23.228, 23.083, 22.934, 22.789, 22.645, 22.499, 22.351, 22.205, + 22.057, 21.913, 21.767, 21.628, 21.484, 21.337, 21.191, 21.046, 20.9, 20.758, 20.618, 20.476, 20.33, 20.191, + 20.049, 19.908, 19.766, 19.632, 19.489, 19.353, 19.217, 19.076, 18.935, 18.797, 18.659, 18.516, 18.374, + 18.227, 18.085, 17.935, 17.789, 17.64, 17.495, 17.344, 17.119, 16.969, 16.823, 16.669, 16.521, 16.371, 16.219, + 16.072, 15.926, 15.775, 15.629, 15.483, 15.341, 15.195, 15.05, 14.901, 14.757, 14.61, 14.463, 14.315, 14.167, + 14.023, 13.871, 13.726, 13.582, 13.436, 13.291, 13.147, 13.007, 12.863, 12.725, 12.586, 12.446, 12.31, 12.175, + 12.043, 11.898, 11.76, 11.621, 11.473, 11.259, 11.114, 10.97, 10.823, 10.671, 10.523, 10.375, 10.233, 10.082, + 9.935, 9.788, 9.642, 9.494, 9.342, 9.194, 9.046, 8.896, 8.747, 8.598, 8.451, 8.302, 8.158, 8.012, 7.864, + 7.718, 7.569, 7.425, 7.28, 7.132, 6.986, 6.841, 6.693, 6.543, 6.396, 6.246, 6.099, 5.95, 5.801, 5.653, 5.5, + 5.351, 5.2, 4.974, 4.821, 4.669, 4.522, 4.373, 4.222, 4.068, 3.918, 3.77, 3.623, 3.472, 3.322, 3.173, 3.025, + 2.882, 2.733, 2.583, 2.436, 2.289, 2.144, 1.994, 1.845, 1.695, 1.546, 1.397, 1.245, 1.091, 0.939, 0.791, + 0.641, 0.493, 0.346, 0.197, 0.051, -0.102, -0.254, -0.404, -0.553, -0.708, -0.859, -1.012, -1.166, -1.321, + -1.474, -1.628, -1.858, -2.012, -2.164, -2.32, -2.473, -2.626, -2.776, -2.927, -3.078, -3.231, -3.384, -3.535, + -3.686, -3.836, -3.987, -4.136, -4.287, -4.43, -4.58, -4.728, -4.872, -5.013, -5.158, -5.302, -5.442, -5.585, + -5.728, -5.873, -6.015, -6.158, -6.305, -6.447, -6.59, -6.733, -6.877, -7.022, -7.165, -7.307, -7.449, -7.59, + -7.732, -7.871, -8.012, -8.151, -8.29, -8.429, -8.576, -8.718, -8.863, -9.006, -9.153, -9.371, -9.514, -9.657, + -9.789, -9.937, -10.083, -10.229, -10.378, -10.523, -10.668, -10.812, -10.958, -11.103, -11.247, -11.395, + -11.54, -11.689, -11.832, -11.978, -12.121, -12.267, -12.412, -12.559, -12.705, -12.85, -13.006, -13.15, + -13.3, -13.447, -13.593, -13.741, -13.889, -14.033, -14.182, -14.326, -14.474, -14.607, -14.754, -14.903, + -15.051, -15.195, -15.346, -15.491, -15.642, -15.794, -15.945, -16.091, -16.249, -16.401, -16.546, -16.696, + -16.843, -16.986, -17.132, -17.272, -17.413, -17.553, -17.694, -17.839, -17.984, -18.128, -18.271, -18.413, + -18.558, -18.705, -18.857, -19.008, -19.149, -19.293, -19.441, -19.586, -19.73, -19.876, -20.023, -20.169, + -20.317, -20.462, -20.61, -20.763, -20.912, -21.067, -21.225, -21.373, -21.528, -21.675, -21.818, -21.953, + -22.076, -22.203, -22.372, -22.481, -22.601, -22.718, -22.832, -22.95, -23.07, -23.192, -23.315, -23.439, + -23.56, -23.68, -23.799, -23.912, -24.022, -24.132, -24.242, -24.34, -24.435, -24.518, -24.605, -24.686, + -24.761, -24.838, -24.911, -24.978, -25.039, -25.1, -25.154, -25.213, -25.253, -25.307, -25.361, -25.408, + -25.451, -25.493, -25.53, -25.573, -25.62, -25.642, -25.665, -25.683, -25.703, -25.718, -25.732, -25.747, + -25.758, -25.759, -25.769, -25.778, -25.789, -25.799, -25.799, -25.8, -25.8, -25.8, -25.8, -25.807, -25.816, + -25.823, -25.836, -25.841, -25.861, -25.884, -25.908, -25.936, -25.961, -25.99, -26.017, -26.048, -26.084, + -26.118, -26.141, -26.176, -26.213, -26.247, -26.274, -26.299, -26.34, -26.373, -26.403, -26.435, -26.467, + -26.5, -26.532, -26.563, -26.599, -26.624, -26.661, -26.683, -26.715, -26.748, -26.78, -26.813, -26.842, + -26.873, -26.908, -26.943, -26.977, -27.008, -27.043, -27.068, -27.095, -27.133, -27.164, -27.193, -27.224, + -27.255, -27.287, -27.319, -27.348, -27.381, -27.407, -27.448, -27.483, -27.518, -27.55, -27.587, -27.615, + -27.643, -27.673, -27.697, -27.725, -27.753, -27.782, -27.823, -27.849, -27.876, -27.901, -27.931, -27.955, + -27.982, -28.009, -28.036, -28.065, -28.088, -28.117, -28.142, -28.172, -28.201, -28.232, -28.251, -28.278, + -28.304, -28.333, -28.361, -28.38, -28.403, -28.436, -28.464, -28.495, -28.531, -28.558, -28.581, -28.618, + -28.649, -28.679, -28.711, -28.74, -28.769, -28.792, -28.825, -28.851, -28.877, -28.901, -28.931, -28.955, + -28.984, -29.007, -29.039, -29.063, -29.097, -29.129, -29.158, -29.18, -29.206, -29.227, -29.251, -29.28, + -29.304, -29.328, -29.346, -29.369, -29.384, -29.409, -29.426, -29.458, -29.479, -29.499] + target_speed = 5 / 3.6 # [m/s] + T = 500.0 # 最大模拟时间 + # 设置车辆的出事状态 + state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed) + # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0) + lastIndex = len(cx) - 1 + time = 0.0 + x = [state.x] + # print('x',x) + y = [state.y] + # print('y',y) + yaw = [state.yaw] + v = [state.v] + t = [0.0] + target_ind = calc_target_index(state, cx, cy) + while True: + # print('heading',tcp4.heading) + ai = PControl(target_speed, state.v) + di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) + state = update(state, ai, di) + time = time + dt + x.append(state.x) + x1 = x[-1] + print('x1', x[-1]) + y.append(state.y) + y1 = y[-1] + print('y1', y[-1]) + yaw.append(state.yaw) + yaw1 = y[-1] * 57.3 + print('yaw', yaw[-1]) + t.append(time) + # logger.add('file.log') + # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north)) + v.append(state.v) + # print('v', v[-1]) + + t.append(time) + # plt.cla() + # plt.plot(cx, cy, ".r", label="course") + # plt.plot(x, y, "-b", label="trajectory") + # plt.plot(cx[target_ind], cy[target_ind], "go", label="target") + # plt.axis("equal") + # plt.grid(True) + # plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4]) + # plt.pause(0.001) + + +def car_back_right(): + + print('进入右倒库') + while True: + global fangxp + db_file = "turndb.db" + conn = sqlite3.connect(db_file) + cur = conn.cursor() + # sql='INSERT INTO turndb(id) VALUES(1);' + sql = "update turndb set angle='%s' WHERE id = 1" % fangxp + cur.execute(sql) + conn.commit() + cur.close() + conn.close() + # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2)) + + ra_trace = abs(math.sqrt(pow((float(tcp4.east)-68.35),2) + pow((float(tcp4.north)-43.85),2))) + print('a_trace',ra_trace) + rb_trace = abs(math.sqrt((float(tcp4.east)-70.15) ** 2 + (float(tcp4.north)-38.6)** 2)) + print('b_trace',rb_trace) + rc_trace = abs(math.sqrt((float(tcp4.east)-70.7) ** 2 + (float(tcp4.north)-38.4) ** 2)) + print('c_trace',rc_trace) + rd_trace = abs(math.sqrt((float(tcp4.east)-72) ** 2 + (float(tcp4.north)-37.8) ** 2)) + print('d_trace',rd_trace) + # print("tcp4.east",tcp4.east) + print('tcp_fangxiang',tcp4.fang_xiang) + + if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and ra_trace<1.2: + fangxp = -540 + + print('右打死',fangxp) + + elif 70< float(tcp4.fang_xiang)< 110 and rb_trace<1.2: + fangxp = 0 + + print('回正',fangxp) + + elif 70 < float(tcp4.fang_xiang) < 110 and rc_trace<1.2: + fangxp = -540 + print('右打死',fangxp) + + + elif 70< float(tcp4.fang_xiang) < 90 and rd_trace<1.2: + fangxp = 0 + print('回正',fangxp) + else: + print('lose') + + print('\n') + time.sleep(0.5) + +def car_back_right_fly(): + + print('进入右出库') + while True: + global fangxp + db_file = "turndb.db" + conn = sqlite3.connect(db_file) + cur = conn.cursor() + # sql='INSERT INTO turndb(id) VALUES(1);' + sql = "update turndb set angle='%s' WHERE id = 1" % fangxp + cur.execute(sql) + conn.commit() + cur.close() + conn.close() + # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2)) + + rfa_trace = abs(math.sqrt(pow((float(tcp4.east)-72.02),2) + pow((float(tcp4.north)-37.65),2))) + print('a_trace',rfa_trace) + rfb_trace = abs(math.sqrt((float(tcp4.east)-68.84) ** 2 + (float(tcp4.north)-43.36)** 2)) + print('b_trace',rfb_trace) + print('tcp_fangxiang',tcp4.fang_xiang) + + if 60.0 < (float(tcp4.fang_xiang)) < 100.0 and rfa_trace<1.2: + fangxp = -540 + + print('右打死',fangxp) + + elif 145.0< float(tcp4.fang_xiang) < 185.0 and rfb_trace<1.2: + fangxp = 0 + + print('回正',fangxp) + else: + print('lose') + + print('\n') + time.sleep(0.5) +def car_back_lelf(): + + print('进入左倒库') + while True: + global fangxp + db_file = "turndb.db" + conn = sqlite3.connect(db_file) + cur = conn.cursor() + # sql='INSERT INTO turndb(id) VALUES(1);' + sql = "update turndb set angle='%s' WHERE id = 1" % fangxp + cur.execute(sql) + conn.commit() + cur.close() + conn.close() + # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2)) + + 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.2: + 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_lelf_fly(): + + print('进入左出库') + while True: + global fangxp + db_file = "turndb.db" + conn = sqlite3.connect(db_file) + cur = conn.cursor() + # sql='INSERT INTO turndb(id) VALUES(1);' + sql = "update turndb set angle='%s' WHERE id = 1" % fangxp + cur.execute(sql) + conn.commit() + cur.close() + conn.close() + # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2)) + + lfa_trace = abs(math.sqrt(pow((float(tcp4.east)-72.02),2) + pow((float(tcp4.north)-37.51),2))) + # print('a_trace',a_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) + else: + print('lose') + + print('\n') + time.sleep(0.5) + + + +if __name__ == '__main__': + # main() + t1 = threading.Thread(target=test5.can165) # 读取eps状态 + t2 = threading.Thread(target=test5.can0a5) # 读取目前方向盘角度 + t3 = threading.Thread(target=test5.main) + t4 = threading.Thread(target=test5.update) + t5 = threading.Thread(target=car_back_right) + t6 = threading.Thread(target=tcp4.zuo_biao) + t7 = threading.Thread(target=sped.sped) + t8 = threading.Thread(target=car_back_right_fly) + t9 = threading.Thread(target=car_back_lelf) + t10 = threading.Thread(target=car_back_lelf_fly) + # 启动线程 + # t5.start() + t8.start() + # t9.start() + # t10.start() + t6.start() + t7.start() + t1.start() + t2.start() + t3.start() + t4.start() + diff --git a/sped.py b/sped.py new file mode 100644 index 0000000..e95b2d1 --- /dev/null +++ b/sped.py @@ -0,0 +1,41 @@ +import os +import datetime + +import can #需要安装python-can库 pip install python-can +from can import bus + +speed=0 +def sped(): + global speed + bus.set_filters=[{"can_id": 0x4f2, "can_mask": 0xFFFF, "extended": False}] + #连接can1 + + can1 = can.interface.Bus(channel = 'can0', bustype = 'socketcan',can_filters=bus.set_filters) #python-can 4.0需要用这个 + # can1 = can.interface.Bus(channel = 'can0', bustype = 'socketcan') #python-can 4.0需要用这个 + #一直循环接收数据 + a = 1 + while a == 1 : + msg = can1.recv(3.0) #recv()中定义超时接收时间 + # print(msg) + b = str(msg).replace(' ', '') + c = b.find('ID') + d = b.find('DL') + e = b[c + 3:c + 7] # id + # print(b, '\n') + if e == '04f2': + # print('成功接收到速度报文：04f2 接收时间为：',datetime.datetime.now()) + speed16 = b[d + 6:d + 10] + # print('接收到的速度16进制为：', speed16) # 速度 + speed10 = int(speed16, 16) + speed = speed10 * 0.01 + # print('接收到的速度10进制为：', speed10) + # print('接收到的表显速度为：%skm/h' % speed, '\n') + # file = open('speed.txt', 'a') + # file.write(str(speed)) + # file.write('\n') + + else: + print('未接收到速度报文') + + #关闭can + # os.system('sudo ifconfig can1 down') \ No newline at end of file diff --git a/tcp4.py b/tcp4.py new file mode 100644 index 0000000..ae1d1bf --- /dev/null +++ b/tcp4.py @@ -0,0 +1,130 @@ +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 +car_yaw = 0 +x_car = 0 +y_car = 0 +heading = 0 +east=0 +north=0 +beidou=0 +fang_xiang=0 + +def zuo_biao(): + global car_yaw, x_car, y_car,east,north,beidou,fang_xiang + 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] # 海拔 + + car_heading = beidou[5] # 方位角 + fang_xiang=beidou[5] + car_heading = float(car_heading) + car_heading = car_heading + 180 + + if car_heading > 360: + car_heading = car_heading - 360 + if 0 < car_heading <= 90: + car_yaw = math.radians(90 - float(car_heading)) + car_yaw = round(car_yaw, 3) + + else: + car_yaw = math.radians(450 - float(car_heading)) + car_yaw = round(car_yaw, 3) + # print("car_yaw", car_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] # 后天线使用卫星数 前天线当前参与解算的卫星数量 + + east = beidou[14] # 东向位置坐标：以基站为原点的地理坐标系下的东向位置，单位：米，小数点后 3 位 + x_car = np.cos(car_yaw) * 2.3 + float(east) + x_car = round(x_car, 3) + # print('x',x_car) + north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位 + y_car = -np.sin(car_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() diff --git a/test5.py b/test5.py new file mode 100644 index 0000000..a011fc3 --- /dev/null +++ b/test5.py @@ -0,0 +1,398 @@ +import threading +import time +import os +import datetime +import datetime +import turndb +import can #需要安装python-can库 pip install python-can +from can import bus + +def update(): + global angle_target + while True: + date = turndb.table_quer() + angle_target=int(date[0][1]) + time.sleep(0.01) +def add_0x(initial): + global initial16 + initial16 = '0x%s' % initial + return initial16 + + +def conversion(data10): + global data16 + data100 = hex(data10)[2:].upper() + if len(str(data100)) == 1: + data16 = "0%s" % data100 + else: + data16 = data100 + return data16 + + +def angle_separation(angle): + global angle_low, angle_high, angle1 + angle1 = int(angle / 0.0625) + + buma = angle1 & 0xFFFF # 补码 + buma16 = hex(buma) + if angle1 >= 0: + fanma = buma + else: + fanma = buma - 1 + fanma16 = hex(fanma) + # + angle_all = fanma16[2:].upper() + # angle_all = buma16[2:].upper() + if len(str(angle_all)) == 1: + angle_low = "0%s" % angle_all + angle_high = '00' + elif len(str(angle_all)) == 2: + angle_low = angle_all + angle_high = '00' + elif len(str(angle_all)) == 3: + angle_low = angle_all[1: 3] + angle_high = "0%s" % angle_all[0: 1] + elif len(str(angle_all)) == 4: + angle_low = angle_all[2: 4] + angle_high = angle_all[0: 2] + + # print("换算角度：" + str(angle)) + # print("总：" + angle_all) + # print("低位：" + angle_low) + # print("高位：" + angle_high) + return angle_low, angle_high, angle1 + + +def can165(): + global eps_statue, can_id + bus.set_filters = [{"can_id": 0x0165, "can_mask": 0xFFFF, "extended": False}] + # 连接can1 + + can1 = can.interface.Bus(channel='can1', bustype='socketcan', can_filters=bus.set_filters) # python-can 4.0需要用这个 + # can1 = can.interface.Bus(channel = 'can0', bustype = 'socketcan') #python-can 4.0需要用这个 + # 一直循环接收数据 + + while True: + msg = can1.recv(3.0) # recv()中定义超时接收时间 + # print(msg) + can1651 = str(msg).replace(' ', '') + can165id = can1651.find('ID') + can165dl = can1651.find('DL') + can_id = can1651[can165id + 3:can165id + 7] # id + # print(can1651, '\n') + eps_statue = can1651[can165dl + 4:can165dl + 5] # eps状态 + eps_check = can1651[can165dl + 6:can165dl + 8] # eps保护算法值 + # print('eps状态：', eps_statue) + # return eps_statue, can_id + # time.sleep(0.015) + + +def can0a5(): + global angle_low, angle_high, angle_now + bus.set_filters = [{"can_id": 0x0a5, "can_mask": 0xFFFF, "extended": False}] + # 连接can1 + + can1 = can.interface.Bus(channel='can1', bustype='socketcan', can_filters=bus.set_filters) # python-can 4.0需要用这个 + # can1 = can.interface.Bus(channel = 'can0', bustype = 'socketcan') #python-can 4.0需要用这个 + # 一直循环接收数据 + + while True: + msg = can1.recv(3.0) # recv()中定义超时接收时间 + # print(msg) + can0a51 = str(msg).replace(' ', '') + can0a5id = can0a51.find('ID') + can0a5dl = can0a51.find('DL') + can_id = can0a51[can0a5id + 3:can0a5id + 7] # id + # print(can0a51, '\n') + # time.sleep(0.015) + if can_id == '00a5': + # print('接收到转向角报文0a5') + angle16_receive = can0a51[can0a5dl + 4:can0a5dl + 8] # 获取角度信息16进制 + # print(angle16_receive) + # 分离角度的高低子节 + angle_low = can0a51[can0a5dl + 6:can0a5dl + 8] + angle_high = can0a51[can0a5dl + 4:can0a5dl + 6] + # print(angle_high, angle_low) + ''' + 转换成10进制 + ''' + angle10_receive = int(angle16_receive, 16) + if angle10_receive & 0x8000 == 0x8000: + angle10_receive = -(angle10_receive - 1 ^ 0xffff) + angle_now = angle10_receive * 0.1 + # print('此时方向盘角度为：%s' % angle_now) + + else: + print('未接收到转向角报文0a5') + # return angle_high, angle_low, angle_now + + +def turn(): + global rolling_count + xcag = abs(angle_target - angle_now) + angle_target_temp=angle_target + if xcag >= 13: + print("超出10°") + if angle_target >= angle_now: + while True: + if int(angle_target_temp)!=int(angle_target): + break + elif int(angle_now)int(angle_target_temp): + # print(i) + changeag = angle_now - chajiao + print("当前角度为:", angle_now) + print("变换角度：", changeag) + print("临时目标角为：", angle_target_temp) + angle_separation(changeag) + # print("变换后角度", angle1) + # print("角度高低字节",angle_high,angle_low) + + # 总合 + if rolling_count == 16: + rolling_count = 0 + rolling_count16 = hex(rolling_count)[2:].upper() + # angle_separation(angle) + # print(angle_low,angle_high) + # 字符串转16进制 + rolling_count161 = int('%s%s' % (add_0x(rolling_count16), Angle_request), 16) + angle_high16 = int(add_0x(angle_high), 16) + angle_low16 = int(add_0x(angle_low), 16) + # print("前三字节16进制：", '%s%s' % (add_0x(rolling_count16), Angle_request), add_0x(angle_high), + # add_0x(angle_low)) + # print("前三字节10进制：", rolling_count161, angle_high16, angle_low16) + + # 按位异或 10进制 + protection_value10 = rolling_count161 ^ angle_high16 ^ angle_low16 + # print("校验值10进制：",protection_value10) + # 高低子节转换 + protection_value16 = conversion(protection_value10) + # print("校验值16进制：", protection_value16) + print( + 'cansend can1 160#%s%s%s%s%s00000000' % (rolling_count16, Angle_request, angle_high, angle_low, + protection_value16)) + os.system( + 'cansend can1 160#%s%s%s%s%s00000000' % (rolling_count16, Angle_request, angle_high, angle_low, + protection_value16)) + print('\n') + rolling_count = rolling_count + 1 + time.sleep(0.015) + else: + break + + + + else: + changeag = angle_target_temp + print("当前角度为:", angle_now) + print("变换角度：", changeag) + print('临时目标角为：',angle_target_temp) + # print(changeag) + angle_separation(changeag) + # print("变换后角度", angle1) + # print(angle_low,angle_high) + # 总合 + if rolling_count == 16: + rolling_count = 0 + rolling_count16 = hex(rolling_count)[2:].upper() + # angle_separation(angle) + # print(angle_low,angle_high) + # 字符串转16进制 + rolling_count161 = int('%s%s' % (add_0x(rolling_count16), Angle_request), 16) + angle_high16 = int(add_0x(angle_high), 16) + angle_low16 = int(add_0x(angle_low), 16) + # print("前三字节16进制：", '%s%s' % (add_0x(rolling_count16), Angle_request), add_0x(angle_high), add_0x(angle_low)) + # print("前三字节10进制：", rolling_count161, angle_high16, angle_low16) + + # 按位异或 10进制 + protection_value10 = rolling_count161 ^ angle_high16 ^ angle_low16 + # print("校验值10进制：",protection_value10) + # 高低子节转换 + protection_value16 = conversion(protection_value10) + # print("校验值16进制：", protection_value16) + + print('cansend can1 160#%s%s%s%s%s00000000' % (rolling_count16, Angle_request, angle_high, angle_low, + protection_value16)) + os.system('cansend can1 160#%s%s%s%s%s00000000' % (rolling_count16, Angle_request, angle_high, angle_low, + protection_value16)) + print('\n') + rolling_count = rolling_count + 1 + time.sleep(0.015) + + +def turn_keep(): + global rolling_count + angle_separation(angle_now) + if rolling_count == 16: + rolling_count = 0 + rolling_count16 = hex(rolling_count)[2:].upper() + # angle_separation(angle) + # print(angle_low,angle_high) + # 字符串转16进制 + rolling_count161 = int('%s%s' % (add_0x(rolling_count16), 0), 16) + angle_high16 = int(add_0x(angle_high), 16) + angle_low16 = int(add_0x(angle_low), 16) + print("前三字节16进制：", '%s%s' % (add_0x(rolling_count16), 0), add_0x(angle_high), + add_0x(angle_low)) + # print("前三字节10进制：", rolling_count161, angle_high16, angle_low16) + + # 按位异或 10进制 + protection_value10 = rolling_count161 ^ angle_high16 ^ angle_low16 + # print("校验值10进制：",protection_value10) + # 高低子节转换 + protection_value16 = conversion(protection_value10) + print("校验值16进制：", protection_value16) + print('cansend can1 160#%s0%s%s%s00000000' % (rolling_count16, angle_high, angle_low, + protection_value16)) + os.system('cansend can1 160#%s0%s%s%s00000000' % (rolling_count16, angle_high, angle_low, + protection_value16)) + print('\n') + rolling_count = rolling_count + 1 + time.sleep(0.015) +def turn_keep2(): + global rolling_count + angle_separation(angle_now) + if rolling_count == 16: + rolling_count = 0 + rolling_count16 = hex(rolling_count)[2:].upper() + # angle_separation(angle) + # print(angle_low,angle_high) + # 字符串转16进制 + rolling_count161 = int('%s%s' % (add_0x(rolling_count16), Angle_request), 16) + angle_high16 = int(add_0x(angle_high), 16) + angle_low16 = int(add_0x(angle_low), 16) + # print("前三字节16进制：", '%s%s' % (add_0x(rolling_count16), Angle_request), add_0x(angle_high), + # add_0x(angle_low)) + # print("前三字节10进制：", rolling_count161, angle_high16, angle_low16) + + # 按位异或 10进制 + protection_value10 = rolling_count161 ^ angle_high16 ^ angle_low16 + # print("校验值10进制：",protection_value10) + # 高低子节转换 + protection_value16 = conversion(protection_value10) + # print("校验值16进制：", protection_value16) + print('cansend can1 160#%s1%s%s%s00000000' % (rolling_count16, angle_high, angle_low, + protection_value16)) + os.system('cansend can1 160#%s1%s%s%s00000000' % (rolling_count16, angle_high, angle_low, + protection_value16)) + print('\n') + rolling_count = rolling_count + 1 + time.sleep(0.015) + +def main(): + while True: + global can_id + #can165() + if can_id == '0165': + ''' + eps_statue=c 驾驶员未干预方向盘；驾驶员干预方向盘检测有效；eps永久失效 + eps_statue=0 驾驶员未干预方向盘；驾驶员干预方向盘检测有效；eps暂时不可控 + eps_statue=4 驾驶员未干预方向盘；驾驶员干预方向盘检测有效；eps可控 + eps_statue=8 驾驶员未干预方向盘；驾驶员干预方向盘检测有效；eps正处于被控 + + eps_statue=d 驾驶员干预方向盘；驾驶员干预方向盘检测有效；eps永久失效 + eps_statue=0 驾驶员干预方向盘；驾驶员干预方向盘检测有效；eps暂时不可控 + eps_statue=5 驾驶员干预方向盘；驾驶员干预方向盘检测有效；eps可控 + eps_statue=9 驾驶员干预方向盘；驾驶员干预方向盘检测有效；eps正处于被控 + ''' + # print('接收到EPS报文：0165') + + if eps_statue == '0': + print("eps暂时不可控") + # can0a5() + print("当前角度为:", angle_now) + turn_keep() + + elif eps_statue == '4': + print("当前角度为:", angle_now) + print("eps可控") + turn_keep2() + elif eps_statue == '8': + print('eps正处于被控') + # update() + if angle_now == angle_target: + print('目标角度和当前角度相同',angle_now) + turn_keep2() + else: + # can0a5() + turn() + print("当前角度为:",angle_now) + elif eps_statue == 'c': + kk=1 + print('eps永久失效,请重启') + else: + print('eps正处于其他状态详见报文：', eps_statue) + else: + print('未接收到165报文') + + +rolling_count = 0 +angle_high = '00' +angle_low = '00' +# protection_value=10 +# angle=-2047.9375 #调整转向角 +# angle = 30 # 调整转向角 +angle_target = 0 +angle_now = 0 # 当前 +# angle1=2047.9375 +Angle_request = 1 +eps_statue = 0 +chajiao=7.9 +can_id ='0165' + +if __name__ == '__main__': + #创建线程 + t1 = threading.Thread(target=can165) # 读取eps状态 + t2 = threading.Thread(target=can0a5) # 读取目前方向盘角度 + t3 = threading.Thread(target=main) + t4=threading.Thread(target=update) + #启动线程 + t1.start() + t2.start() + t3.start() + t4.start() diff --git a/test_angle.py b/test_angle.py new file mode 100644 index 0000000..d478f39 --- /dev/null +++ b/test_angle.py @@ -0,0 +1,14 @@ +import math +heading = 360 +heading=heading+180 +if heading>360: + heading=heading-360 + print('head',heading) + +if 0 < heading <= 90: + yaw = (90 - float(heading)) + print('yaw',yaw) +else: + yaw = (450-float(heading)) + + print('yaw3',yaw) \ No newline at end of file diff --git a/turndb.db b/turndb.db new file mode 100644 index 0000000000000000000000000000000000000000..ba8d4c35d62a7d659abfeb2f9ab79097000abb3c GIT binary patch literal 8192 zcmeI#y$XU*6u|LwOM;-VE!^%{5D^s3SvUkPl`o)ZA((;O-m%ANcIt*WH@GzX5BKXF z?g#v~dpU)<$@KM6-D9KIG7+U@r?trFEh0VJ+fa7lw27U+`H!%7EH`>=cPaNJwh;&* zfB*srAbiG9&DcAf13wCM