Add PID test
This commit is contained in:
parent
d654443621
commit
013768ad1c
2 changed files with 221 additions and 0 deletions
126
controller/fw/embed/test/python_readPID_angle_parametrs.py
Normal file
126
controller/fw/embed/test/python_readPID_angle_parametrs.py
Normal file
|
|
@ -0,0 +1,126 @@
|
|||
import can
|
||||
import time
|
||||
import struct
|
||||
import sys
|
||||
# Конфигурация
|
||||
CAN_INTERFACE = 'can0'
|
||||
DEVICE_ID = int(sys.argv[1]) # ID устройства по умолчанию
|
||||
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()
|
||||
95
controller/fw/embed/test/python_writePID_angle_parametrs.py
Normal file
95
controller/fw/embed/test/python_writePID_angle_parametrs.py
Normal file
|
|
@ -0,0 +1,95 @@
|
|||
import can
|
||||
import time
|
||||
import struct
|
||||
import sys
|
||||
# Конфигурация
|
||||
CAN_INTERFACE = 'can0'
|
||||
DEVICE_ID = int(sys.argv[1]) # Установка id устройства ввод в десятичной системе счисления
|
||||
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__":
|
||||
if len(sys.argv) != 2:
|
||||
print("Используйте python3 pid_set.py addr")
|
||||
sys.exit(1)
|
||||
main()
|
||||
Loading…
Add table
Add a link
Reference in a new issue