rename python test file names

controller/fw/embed/test/readPID_angle_parametrs.py

@@ -0,0 +1,126 @@
+import can
+import time
+import struct
+import sys
+# Конфигурация
+CAN_INTERFACE = 'can0'
+DEVICE_ID = 0x27          # 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()