Add test SimpleFOC
This commit is contained in:
parent
28ce1bb556
commit
e80f04d857
1 changed files with 96 additions and 0 deletions
96
controller/fw/embed/test/test_simpleFOC.py
Normal file
96
controller/fw/embed/test/test_simpleFOC.py
Normal file
|
@ -0,0 +1,96 @@
|
||||||
|
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()
|
Loading…
Add table
Add a link
Reference in a new issue