77 lines
2.2 KiB
Python
77 lines
2.2 KiB
Python
import protocol_kx
|
|
import can
|
|
import cantools
|
|
import os
|
|
import numpy as np
|
|
import struct
|
|
import time
|
|
|
|
|
|
K = np.zeros(protocol_kx.KX_K_COUNT)
|
|
|
|
|
|
class KX:
|
|
def __init__(self, id, value, sbl_id, can):
|
|
K[id] = value
|
|
self.sbl_id = sbl_id
|
|
self.can0 = can
|
|
|
|
|
|
def KXInit(self, id, value):
|
|
K[id] = value
|
|
|
|
|
|
def reset(self):
|
|
resetMsg = can.Message(arbitration_id=0xA, data=[99, self.sbl_id], is_extended_id=False)
|
|
try:
|
|
self.can0.send(resetMsg)
|
|
print(f"Message sent on{self.can0.channel_info}")
|
|
except self.can0.CanError:
|
|
print("Message NOT sent")
|
|
|
|
|
|
|
|
def sendK(self):
|
|
i = 0
|
|
for element in K:
|
|
ba = bytearray(struct.pack("f", element))
|
|
messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, i, 0,
|
|
protocol_kx.KX_FLAG_SEND, ba[0], ba[1], ba[2], ba[3]], is_extended_id=False)
|
|
try:
|
|
self.can0.send(messageK)
|
|
time.sleep(0.01)
|
|
print(f"Message sent on{self.can0.channel_info}")
|
|
except self.can0.CanError:
|
|
print("Message NOT sent")
|
|
i = i + 1
|
|
|
|
|
|
|
|
def writeToFlashK(self):
|
|
msg = protocol_kx.CANMsgK(self.sbl_id, 0, protocol_kx.KX_FLAG_WRITE_REQ, K)
|
|
messageK = can.Message(arbitration_id=0x2BB, data=[self.sbl_id, 0, 0,
|
|
protocol_kx.KX_FLAG_WRITE_REQ], is_extended_id=False)
|
|
try:
|
|
self.can0.send(messageK)
|
|
time.sleep(0.01)
|
|
print(f"Flash sent on{self.can0.channel_info}")
|
|
except self.can0.CanError:
|
|
print("Flash NOT sent")
|
|
|
|
|
|
def send_test_source_enable(self, x, y, xVel, yVel):
|
|
resetMsg = can.Message(
|
|
arbitration_id=0xA,
|
|
data=[
|
|
0x53,
|
|
0xFF,
|
|
struct.pack('b', int(x))[0],
|
|
struct.pack('B', int(y))[0],
|
|
struct.pack('b', int(xVel))[0],
|
|
struct.pack('b', int(yVel))[0],
|
|
0x68,
|
|
0xA4
|
|
],
|
|
is_extended_id=False
|
|
)
|
|
self.can0.send(resetMsg) |