2025-05-27 15:54:04 +03:00
|
|
|
|
import can
|
|
|
|
|
import time
|
|
|
|
|
import struct
|
|
|
|
|
import sys
|
|
|
|
|
# Конфигурация
|
|
|
|
|
CAN_INTERFACE = 'can0'
|
2025-06-03 10:48:29 +03:00
|
|
|
|
DEVICE_ID = int(sys.argv[1]) # ID ADDR for servo
|
2025-05-27 15:54:04 +03:00
|
|
|
|
REG_WRITE = 0x8 # Код команды записи
|
|
|
|
|
REG_MOTOR_POSPID_Kp = 0x30
|
|
|
|
|
REG_MOTOR_POSPID_Ki = 0x31
|
|
|
|
|
REG_MOTOR_POSPID_Kd = 0x32
|
|
|
|
|
|
|
|
|
|
def send_can_message(bus, can_id, data):
|
|
|
|
|
"""Отправка CAN-сообщения"""
|
|
|
|
|
try:
|
|
|
|
|
msg = can.Message(
|
|
|
|
|
arbitration_id=can_id,
|
|
|
|
|
data=data,
|
|
|
|
|
is_extended_id=False
|
|
|
|
|
)
|
|
|
|
|
bus.send(msg)
|
|
|
|
|
print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}")
|
|
|
|
|
return True
|
|
|
|
|
except can.CanError as e:
|
|
|
|
|
print(f"Ошибка CAN: {e}")
|
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
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_pid_value(bus, device_id, reg, value):
|
|
|
|
|
"""Отправка коэффициента PID на устройство"""
|
|
|
|
|
# Формируем CAN ID для записи: (device_id << 4) | REG_WRITE
|
|
|
|
|
can_id_write = (device_id << 4) | REG_WRITE
|
|
|
|
|
|
|
|
|
|
# Упаковываем значение в байты (little-endian)
|
|
|
|
|
float_bytes = struct.pack('<f', value)
|
|
|
|
|
|
|
|
|
|
# Формируем часть данных (регистр + значение)
|
|
|
|
|
data_part = [reg] + list(float_bytes)
|
|
|
|
|
|
|
|
|
|
# Полные данные для расчета CRC: CAN ID + данные
|
|
|
|
|
full_data_for_crc = list(can_id_write.to_bytes(2, 'little')) + data_part
|
|
|
|
|
|
|
|
|
|
# Рассчитываем CRC и разбиваем на байты (little-endian)
|
|
|
|
|
crc = validate_crc16(full_data_for_crc)
|
|
|
|
|
crc_bytes = list(crc.to_bytes(2, 'little'))
|
|
|
|
|
|
|
|
|
|
# Собираем итоговый пакет данных
|
|
|
|
|
can_data = data_part + crc_bytes
|
|
|
|
|
|
|
|
|
|
# Отправляем сообщение
|
|
|
|
|
send_can_message(bus, can_id_write, can_data)
|
|
|
|
|
|
|
|
|
|
def main():
|
|
|
|
|
# Запрос коэффициентов у пользователя
|
|
|
|
|
try:
|
|
|
|
|
p = float(input("Введите коэффициент P: "))
|
|
|
|
|
i = float(input("Введите коэффициент I: "))
|
|
|
|
|
d = float(input("Введите коэффициент D: "))
|
|
|
|
|
except ValueError:
|
|
|
|
|
print("Ошибка: Введите числовые значения.")
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
# Инициализация CAN-интерфейса
|
|
|
|
|
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
# Отправка коэффициентов с задержкой
|
|
|
|
|
send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Kp, p)
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
|
|
send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Ki, i)
|
|
|
|
|
time.sleep(1)
|
|
|
|
|
|
|
|
|
|
send_pid_value(bus, DEVICE_ID, REG_MOTOR_POSPID_Kd, d)
|
|
|
|
|
|
|
|
|
|
finally:
|
|
|
|
|
# Завершение работы с шиной
|
|
|
|
|
bus.shutdown()
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
2025-06-03 10:48:29 +03:00
|
|
|
|
if len(sys.argv) != 2:
|
|
|
|
|
print("Используйте python3 pid_set.py addr")
|
|
|
|
|
sys.exit(1)
|
2025-05-27 15:54:04 +03:00
|
|
|
|
main()
|