126 lines
4.3 KiB
Python
126 lines
4.3 KiB
Python
import can
|
||
import time
|
||
import struct
|
||
import sys
|
||
# Конфигурация
|
||
CAN_INTERFACE = 'can0'
|
||
DEVICE_ID = int(sys.argv[1]) # ID ADDR for servo
|
||
REG_READ = 0x7 # Код команды чтения
|
||
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_read_request(bus, device_id, register):
|
||
"""Отправка запроса на чтение регистра"""
|
||
can_id = (device_id << 4) | REG_READ
|
||
data_part = [register, 0x00]
|
||
|
||
# Расчет CRC для CAN ID (2 байта) + данные
|
||
full_data_for_crc = list(can_id.to_bytes(2, 'little')) + data_part
|
||
crc = validate_crc16(full_data_for_crc)
|
||
crc_bytes = list(crc.to_bytes(2, 'little'))
|
||
|
||
# Формирование итогового пакета
|
||
packet = data_part + crc_bytes
|
||
send_can_message(bus, can_id, packet)
|
||
|
||
def receive_pid_response(bus, timeout=1.0):
|
||
"""Получение и проверка ответа с PID-значением"""
|
||
start_time = time.time()
|
||
while time.time() - start_time < timeout:
|
||
msg = bus.recv(timeout=0.1)
|
||
if msg and msg.arbitration_id == DEVICE_ID:
|
||
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
|
||
|
||
if len(msg.data) < 8:
|
||
print("Ошибка: Слишком короткий ответ")
|
||
return None
|
||
|
||
# Извлечение данных и CRC
|
||
data = msg.data
|
||
received_crc = int.from_bytes(data[-2:], byteorder='little')
|
||
|
||
# Подготовка данных для проверки CRC
|
||
id_bytes = msg.arbitration_id.to_bytes(1, 'little')
|
||
full_data = list(id_bytes) + list(data[:-2])
|
||
|
||
# Проверка CRC
|
||
calc_crc = validate_crc16(full_data)
|
||
if calc_crc != received_crc:
|
||
print(f"Ошибка CRC: ожидалось 0x{calc_crc:04X}, получено 0x{received_crc:04X}")
|
||
return None
|
||
|
||
# Извлечение float значения
|
||
try:
|
||
value = struct.unpack('<f', bytes(data[1:5]))[0]
|
||
return value
|
||
except struct.error:
|
||
print("Ошибка распаковки float")
|
||
return None
|
||
|
||
print("Таймаут ожидания ответа")
|
||
return None
|
||
|
||
def main():
|
||
"""Основная логика чтения PID-коэффициентов"""
|
||
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
||
|
||
try:
|
||
# Чтение коэффициентов с задержкой
|
||
print("\nЧтение Kp...")
|
||
send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Kp)
|
||
kp = receive_pid_response(bus)
|
||
if kp is not None:
|
||
print(f"Текущий Kp: {kp:.3f}")
|
||
|
||
time.sleep(1)
|
||
|
||
print("\nЧтение Ki...")
|
||
send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Ki)
|
||
ki = receive_pid_response(bus)
|
||
if ki is not None:
|
||
print(f"Текущий Ki: {ki:.3f}")
|
||
|
||
time.sleep(1)
|
||
|
||
print("\nЧтение Kd...")
|
||
send_read_request(bus, DEVICE_ID, REG_MOTOR_POSPID_Kd)
|
||
kd = receive_pid_response(bus)
|
||
if kd is not None:
|
||
print(f"Текущий Kd: {kd:.3f}")
|
||
|
||
finally:
|
||
bus.shutdown()
|
||
|
||
if __name__ == "__main__":
|
||
if len(sys.argv) != 2:
|
||
print("Используйте python3 read_pid.py addr")
|
||
sys.exit(1)
|
||
main()
|