Add interrupt
This commit is contained in:
parent
207b889fef
commit
4d6a5f8700
123 changed files with 1181239 additions and 1181234 deletions
|
|
@ -1,98 +1,98 @@
|
|||
import can
|
||||
import struct
|
||||
import time
|
||||
import argparse
|
||||
|
||||
# Константы
|
||||
CAN_INTERFACE = 'can0'
|
||||
DEVICE_ID = 0x27 # ID ADDR for servo
|
||||
REG_WRITE = 0x7
|
||||
REG_POS = 0x72 # MOTOR+ANGLE = 0x72
|
||||
|
||||
def validate_crc16(data):
|
||||
# Calculate CRC16
|
||||
crc = 0xFFFF
|
||||
for byte in data:
|
||||
crc ^= byte
|
||||
for _ in range(8):
|
||||
if crc & 0x0001:
|
||||
crc = (crc >> 1) ^ 0xA001
|
||||
else:
|
||||
crc >>= 1
|
||||
return crc
|
||||
|
||||
|
||||
def receive_response(bus, timeout=1.0):
|
||||
"""Ожидание ответа от устройства"""
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
msg = bus.recv(timeout=0.1)
|
||||
if msg:
|
||||
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
|
||||
return msg
|
||||
print("[Ошибка] Таймаут")
|
||||
return None
|
||||
|
||||
def send_target_angle(bus):
|
||||
# ID and cmd
|
||||
arbitration_id = (DEVICE_ID << 4) | REG_WRITE
|
||||
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
|
||||
|
||||
# cmd + parametrs
|
||||
data_write = [REG_POS]
|
||||
|
||||
|
||||
full_data_for_crc = id_bytes + data_write
|
||||
crc = validate_crc16(full_data_for_crc)
|
||||
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
|
||||
|
||||
# Full packet
|
||||
packet = data_write + crc_bytes
|
||||
|
||||
|
||||
msg = can.Message(
|
||||
arbitration_id=arbitration_id,
|
||||
is_extended_id=False,
|
||||
data=packet
|
||||
)
|
||||
|
||||
bus.send(msg)
|
||||
response = receive_response(bus)
|
||||
|
||||
|
||||
if response:
|
||||
data = response.data
|
||||
|
||||
if len(data) < 4:
|
||||
print("Слишком короткий ответ")
|
||||
|
||||
# Проверяем минимальную длину ответа (данные + CRC)
|
||||
else:
|
||||
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
|
||||
#buff with id and data without CRC
|
||||
full_data = list(id_bytes) + list(data[:-2])
|
||||
print(f"Received full_data: {list(full_data)}")
|
||||
received_crc = int.from_bytes(data[-2:], byteorder='little')
|
||||
#calc CRC
|
||||
calc_crc = validate_crc16(full_data)
|
||||
|
||||
print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
|
||||
if received_crc == calc_crc:
|
||||
# Если CRC совпадает, проверяем структуру ответа:
|
||||
velocity = struct.unpack('<f', bytes(data[1:5]))[0]
|
||||
print(f"Угол: {velocity}")
|
||||
else:
|
||||
print("Ошибка: CRC не совпадает")
|
||||
else:
|
||||
print("Устройство не ответило")
|
||||
|
||||
def main():
|
||||
# Инициализация CAN
|
||||
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
||||
print("CAN шина инициализирована.")
|
||||
|
||||
send_target_angle(bus)
|
||||
|
||||
bus.shutdown()
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
import can
|
||||
import struct
|
||||
import time
|
||||
import argparse
|
||||
|
||||
# Константы
|
||||
CAN_INTERFACE = 'can0'
|
||||
DEVICE_ID = 0x27 # ID ADDR for servo
|
||||
REG_WRITE = 0x7
|
||||
REG_POS = 0x72 # MOTOR+ANGLE = 0x72
|
||||
|
||||
def validate_crc16(data):
|
||||
# Calculate CRC16
|
||||
crc = 0xFFFF
|
||||
for byte in data:
|
||||
crc ^= byte
|
||||
for _ in range(8):
|
||||
if crc & 0x0001:
|
||||
crc = (crc >> 1) ^ 0xA001
|
||||
else:
|
||||
crc >>= 1
|
||||
return crc
|
||||
|
||||
|
||||
def receive_response(bus, timeout=1.0):
|
||||
"""Ожидание ответа от устройства"""
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
msg = bus.recv(timeout=0.1)
|
||||
if msg:
|
||||
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
|
||||
return msg
|
||||
print("[Ошибка] Таймаут")
|
||||
return None
|
||||
|
||||
def send_target_angle(bus):
|
||||
# ID and cmd
|
||||
arbitration_id = (DEVICE_ID << 4) | REG_WRITE
|
||||
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
|
||||
|
||||
# cmd + parametrs
|
||||
data_write = [REG_POS]
|
||||
|
||||
|
||||
full_data_for_crc = id_bytes + data_write
|
||||
crc = validate_crc16(full_data_for_crc)
|
||||
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
|
||||
|
||||
# Full packet
|
||||
packet = data_write + crc_bytes
|
||||
|
||||
|
||||
msg = can.Message(
|
||||
arbitration_id=arbitration_id,
|
||||
is_extended_id=False,
|
||||
data=packet
|
||||
)
|
||||
|
||||
bus.send(msg)
|
||||
response = receive_response(bus)
|
||||
|
||||
|
||||
if response:
|
||||
data = response.data
|
||||
|
||||
if len(data) < 4:
|
||||
print("Слишком короткий ответ")
|
||||
|
||||
# Проверяем минимальную длину ответа (данные + CRC)
|
||||
else:
|
||||
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
|
||||
#buff with id and data without CRC
|
||||
full_data = list(id_bytes) + list(data[:-2])
|
||||
print(f"Received full_data: {list(full_data)}")
|
||||
received_crc = int.from_bytes(data[-2:], byteorder='little')
|
||||
#calc CRC
|
||||
calc_crc = validate_crc16(full_data)
|
||||
|
||||
print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
|
||||
if received_crc == calc_crc:
|
||||
# Если CRC совпадает, проверяем структуру ответа:
|
||||
velocity = struct.unpack('<f', bytes(data[1:5]))[0]
|
||||
print(f"Угол: {velocity}")
|
||||
else:
|
||||
print("Ошибка: CRC не совпадает")
|
||||
else:
|
||||
print("Устройство не ответило")
|
||||
|
||||
def main():
|
||||
# Инициализация CAN
|
||||
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
||||
print("CAN шина инициализирована.")
|
||||
|
||||
send_target_angle(bus)
|
||||
|
||||
bus.shutdown()
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue