96 lines
No EOL
3.3 KiB
Python
96 lines
No EOL
3.3 KiB
Python
import can
|
||
import time
|
||
import sys
|
||
import struct
|
||
|
||
# Конфигурация
|
||
CAN_INTERFACE = 'can0'
|
||
DEVICE_ID = 0x69 # Текущий ID устройства
|
||
REG_READ = 0x7
|
||
REG_WRITE = 0x8
|
||
DATA_TYPE_ANGLE = 0x03
|
||
DATA_TYPE_VELOCITY = 0x04
|
||
DATA_TYPE_TORQUE = 0x05
|
||
|
||
# CRC функция (аналогичная устройству)
|
||
def validate_crc16(data):
|
||
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_can_message(bus, can_id, data):
|
||
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 receive_response(bus, timeout=1.0):
|
||
start_time = time.time()
|
||
while time.time() - start_time < timeout:
|
||
msg = bus.recv(timeout=0.1)
|
||
if msg:
|
||
print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}")
|
||
return msg
|
||
print("[Ошибка] Таймаут")
|
||
return None
|
||
|
||
def test_simplefoc_else_block():
|
||
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
||
|
||
# 1. Установка типа данных (DATA_TYPE_ANGLE)
|
||
can_id_write = (DEVICE_ID << 4) | REG_WRITE
|
||
data_set_type = [DATA_TYPE_ANGLE, 0x00]
|
||
full_data = list(can_id_write.to_bytes(2, 'little')) + data_set_type
|
||
crc = validate_crc16(full_data)
|
||
crc_bytes = list(crc.to_bytes(2, 'little'))
|
||
packet = data_set_type + crc_bytes
|
||
send_can_message(bus, can_id_write, packet)
|
||
time.sleep(0.1) # Ожидание обработки
|
||
|
||
# 2. Отправка SimpleFOC сообщения (угол)
|
||
target_angle = 45.0
|
||
angle_bytes = struct.pack('<f', target_angle)
|
||
can_id_simplefoc = (DEVICE_ID << 4) | 0x01 # Не REG_READ/REG_WRITE
|
||
payload = [0x00] + list(angle_bytes) + [0x00] # [type placeholder, angle, padding]
|
||
|
||
# Расчет CRC
|
||
full_data_sf = list(can_id_simplefoc.to_bytes(2, 'little')) + payload
|
||
crc_sf = validate_crc16(full_data_sf)
|
||
payload += list(crc_sf.to_bytes(2, 'little'))
|
||
|
||
# Отправка
|
||
print("\nТест SimpleFOC (блок else):")
|
||
send_can_message(bus, can_id_simplefoc, payload)
|
||
|
||
# 3. Проверка ответа (отправка угла + установка нового угла)
|
||
response = receive_response(bus)
|
||
if response:
|
||
# Проверка структуры ответа
|
||
if response.data[0] == ord('A'):
|
||
print("Успех: Отправлен текущий угол")
|
||
else:
|
||
print("Ошибка: Неверный тип ответа")
|
||
else:
|
||
print("Ошибка: Нет ответа от устройства")
|
||
|
||
# 4. Проверка установки нового угла (интеграционно)
|
||
# ... (может требовать дополнительной проверки на устройстве)
|
||
|
||
bus.shutdown()
|
||
|
||
if __name__ == "__main__":
|
||
test_simplefoc_else_block() |