67 lines
2.1 KiB
Python
67 lines
2.1 KiB
Python
import can
|
|
import struct
|
|
import time
|
|
import argparse
|
|
|
|
# Константы
|
|
CAN_INTERFACE = 'can0'
|
|
DEVICE_ID = 0x00 # Старый ID устройства
|
|
REG_WRITE = 0x8
|
|
REG_POS = 0x72 # Предположим, что команда записи позиции — 0x2
|
|
|
|
def validate_crc16(data):
|
|
"""Функция расчета CRC16 (MODBUS)"""
|
|
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 send_target_angle(bus, target_angle):
|
|
# ID и команда
|
|
arbitration_id = (DEVICE_ID << 4) | REG_WRITE
|
|
id_bytes = list(arbitration_id.to_bytes(2, byteorder='little'))
|
|
|
|
# Команда и параметры
|
|
data_write = [REG_POS] + list(struct.pack('<f', target_angle)) # Позиция как float (4 байта)
|
|
|
|
# Для CRC — весь пакет: id + команда + параметры
|
|
full_data_for_crc = id_bytes + data_write
|
|
crc = validate_crc16(full_data_for_crc)
|
|
crc_bytes = list(crc.to_bytes(2, byteorder='little'))
|
|
|
|
# Итоговый пакет
|
|
packet = data_write + crc_bytes
|
|
|
|
# Создание и отправка сообщения
|
|
msg = can.Message(
|
|
arbitration_id=arbitration_id,
|
|
is_extended_id=False,
|
|
data=packet
|
|
)
|
|
|
|
try:
|
|
bus.send(msg)
|
|
print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
|
|
except can.CanError:
|
|
print("Ошибка отправки сообщения")
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
|
|
parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
|
|
args = parser.parse_args()
|
|
|
|
# Инициализация CAN
|
|
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
|
print("CAN шина инициализирована.")
|
|
|
|
send_target_angle(bus, args.angle)
|
|
|
|
bus.shutdown()
|
|
|
|
if __name__ == '__main__':
|
|
main()
|