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