servo/controller/fw/embed/test/python_send_angle.py

68 lines
1.8 KiB
Python
Raw Normal View History

import can
import struct
import time
import argparse
2025-05-13 14:43:45 +03:00
# Константы
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x00 # ID ADDR for servo
REG_WRITE = 0x8
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 send_target_angle(bus, target_angle):
2025-05-13 14:43:45 +03:00
# 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] + list(struct.pack('<f', target_angle))
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
)
try:
bus.send(msg)
2025-05-13 14:43:45 +03:00
print(f"[Отправка] CAN ID: 0x{arbitration_id:03X}, Угол: {target_angle} rad, Данные: {list(msg.data)}")
except can.CanError:
2025-05-13 14:43:45 +03:00
print("Ошибка отправки сообщения")
def main():
2025-05-13 14:43:45 +03:00
parser = argparse.ArgumentParser(description="Отправка угла позиции по CAN.")
parser.add_argument("--angle", type=float, required=True, help="Угол (в градусах)")
args = parser.parse_args()
2025-05-13 14:43:45 +03:00
# Инициализация CAN
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
print("CAN шина инициализирована.")
2025-05-13 14:43:45 +03:00
send_target_angle(bus, args.angle)
2025-05-13 14:43:45 +03:00
bus.shutdown()
if __name__ == '__main__':
main()