Skip to content
TheChatArchiveThe Chat Archive
Mistralmistral-medium-2505· Mistral AIHugging Face LMArena Human Preference 140K

/* USER CODE BEGIN Includes */ #include <stdio.h> #include "uartRingBufDMA.h" #include…

From a Chatbot Arena battleThe same person sent this prompt to two anonymous models at once. Compare it with Claudeclaude-3-7-sonnet-20250219-thinking-32k’s answer.
  1. User

    /* USER CODE BEGIN Includes */ #include <stdio.h> #include "uartRingBufDMA.h" #include "stdbool.h" #include "stdlib.h" #include "wheelslib.h" #include "string.h" #include "pid.h"

    extern uint16_t Head, Tail; extern uint8_t RxBuf[RxBuf_SIZE]; extern uint8_t MainBuf[MainBuf_SIZE];

    #define MAX_BUTTONS 8 #define Enc_A_Pin GPIO_PIN_0 #define Enc_B_Pin GPIO_PIN_1 #define Enc_C_Pin GPIO_PIN_8 #define Enc_D_Pin GPIO_PIN_9

    volatile int32_t encA_count = 0; volatile int32_t encB_count = 0; volatile int32_t encC_count = 0; volatile int32_t encD_count = 0;

    typedef struct { volatile int32_t *counter; uint16_t pin; } EncSensor;

    EncSensor sensors[4] = { {&encA_count, Enc_A_Pin}, {&encB_count, Enc_B_Pin}, {&encC_count, Enc_C_Pin}, {&encD_count, Enc_D_Pin} };

    uint8_t pwmIndex = 0; char buffer[100]; uint32_t pwmData_CH1[10] = {0}; uint32_t pwmData_CH2[10] = {0}; uint32_t pwmData_CH3[10] = {0}; uint32_t pwmData_CH4[10] = {0};

    // Структуры данных для очереди typedef enum { PACKET_TYPE_BUTTON = 0xF0, PACKET_TYPE_JOYSTICK = 0xF1 } PacketType;

    typedef struct { PacketType type; union { struct { uint8_t id; uint8_t state; } button; struct { uint8_t x; uint8_t y; uint8_t power; } joystick; } data; } PacketMessage;

    // Дескриптор очереди пакетов osMessageQueueId_t packetQueueHandle; #define PACKET_QUEUE_SIZE 16

    /**************************************/ // Для отладки volatile uint32_t total_bytes_received = 0; volatile uint32_t valid_packets_received = 0; volatile uint32_t button_packets_received = 0; volatile uint32_t joystick_packets_received = 0; volatile uint32_t invalid_packets_detected = 0; volatile uint32_t dropped_packets = 0; volatile uint8_t last_joystick_data[3] = {0}; volatile uint8_t last_button_data[2] = {0};

    int16_t joystickPower = 0; char joystickDir = 'F'; /* USER CODE END Includes */

    /* USER CODE BEGIN PV */ // флаг для управления режимом работы typedef enum { CONTROL_MODE_DIRECT, CONTROL_MODE_PID } ControlMode;

    ControlMode currentControlMode = CONTROL_MODE_DIRECT;

    // Структура для хранения желаемых скоростей (setpoints) typedef struct { double speedFL; double speedFR; double speedBL; double speedBR; bool isActive; } TargetSpeeds;

    TargetSpeeds targetSpeeds = {0, 0, 0, 0, false};

    // PID структуры для каждого колеса PID_TypeDef pidFL, pidFR, pidBL, pidBR;

    // Заданные скорости для каждого колеса (setpoints) double setpointFL = 0, setpointFR = 0, setpointBL = 0, setpointBR = 0; double currentSpeedFL = 0, currentSpeedFR = 0, currentSpeedBL = 0, currentSpeedBR = 0; double pidOutputFL = 0, pidOutputFR = 0, pidOutputBL = 0, pidOutputBR = 0;

    // Константы PID - делаем более консервативными #define KP 1.0f // Вместо 0.2f #define KI 0.05f // Вместо 0.01f #define KD 0.01f // Можно оставить #define MOTOR_DEADZONE_MIN 1 #define MOTOR_DEADZONE_MAX 5 #define PID_UPDATE_PERIOD 10

    bool pidDebugEnabled = true; int8_t ticks_per_rev = 20; // Количество тактов кулера в один оборот. float rpm = 0.0;

    // Для расчета скорости - добавляем более детальную диагностику volatile int32_t lastEncA = 0, lastEncB = 0, lastEncC = 0, lastEncD = 0; float ticksToSpeed = 10.0f;

    // Добавляем диагностические переменные volatile uint32_t pidComputeCount = 0; volatile uint32_t pidLastComputeTime = 0; volatile bool pidInitialized = false;

    MotorsState motors; /* USER CODE END PV */

    /* USER CODE BEGIN PFP */ int _write(int file, char ptr, int len) { int i; HAL_UART_Transmit(&huart6, (uint8_t) ptr, len, 50); for (i = 0; i < len; i++) { ITM_SendChar(*ptr++); } return len; }

    void process_uart_data(void); /* USER CODE END PFP */

    /* USER CODE BEGIN 0 */ // Определение состояний FSM для обработки данных из UART typedef enum { STATE_WAIT_HEADER, STATE_COLLECT_BUTTON, STATE_COLLECT_JOYSTICK } RxState;

    void UART_ProcessByte(uint8_t byte) { static RxState state = STATE_WAIT_HEADER; static PacketMessage packet; static uint8_t byteCounter = 0;

    total_bytes_received++;
    
    switch (state) {
        case STATE_WAIT_HEADER:
            if (byte == PACKET_TYPE_BUTTON) {
                packet.type = PACKET_TYPE_BUTTON;
                state = STATE_COLLECT_BUTTON;
                byteCounter = 0;
            } else if (byte == PACKET_TYPE_JOYSTICK) {
                packet.type = PACKET_TYPE_JOYSTICK;
                state = STATE_COLLECT_JOYSTICK;
                byteCounter = 0;
            } else {
                invalid_packets_detected++;
            }
            break;
    
        case STATE_COLLECT_BUTTON:
            if (byteCounter == 0) {
                packet.data.button.id = byte;
                byteCounter++;
            } else if (byteCounter == 1) {
                packet.data.button.state = byte;
                if (osMessageQueuePut(packetQueueHandle, &packet, 0, 0) == osOK) {
                    button_packets_received++;
                    valid_packets_received++;
                    last_button_data[0] = packet.data.button.id;
                    last_button_data[1] = packet.data.button.state;
                } else {
                    dropped_packets++;
                }
                state = STATE_WAIT_HEADER;
            }
            break;
    
        case STATE_COLLECT_JOYSTICK:
            if (byteCounter == 0) {
                packet.data.joystick.x = byte;
                byteCounter++;
            } else if (byteCounter == 1) {
                packet.data.joystick.y = byte;
                byteCounter++;
            } else if (byteCounter == 2) {
                packet.data.joystick.power = byte;
                if (osMessageQueuePut(packetQueueHandle, &packet, 0, 0) == osOK) {
                    joystick_packets_received++;
                    valid_packets_received++;
                    last_joystick_data[0] = packet.data.joystick.x;
                    last_joystick_data[1] = packet.data.joystick.y;
                    last_joystick_data[2] = packet.data.joystick.power;
                } else {
                    dropped_packets++;
                }
                state = STATE_WAIT_HEADER;
            }
            break;
    }
    

    }

    void InitializePID(void) { printf("=== INITIALIZING PID CONTROLLERS ===\r\n");

    // Инициализация PID-регуляторов для каждого колеса
    printf("Initializing PID for FL wheel...\r\n");
    PID(&pidFL, &currentSpeedFL, &pidOutputFL, &setpointFL, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);
    PID(&pidFR, &currentSpeedFR, &pidOutputFR, &setpointFR, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);
    PID(&pidBL, &currentSpeedBL, &pidOutputBL, &setpointBL, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);
    PID(&pidBR, &currentSpeedBR, &pidOutputBR, &setpointBR, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT);
    
    // ИСПРАВЛЕНИЕ: Устанавливаем правильные ограничения выхода
    printf("Setting output limits to ±15...\r\n");  // Исправляем лог
    PID_SetOutputLimits(&pidFL, -15, 15);  // Уменьшаем с ±80 до ±15
    PID_SetOutputLimits(&pidFR, -15, 15);
    PID_SetOutputLimits(&pidBL, -15, 15);
    PID_SetOutputLimits(&pidBR, -15, 15);
    
    // Включаем автоматический режим
    PID_SetMode(&pidFL, _PID_MODE_AUTOMATIC);
    PID_SetMode(&pidFR, _PID_MODE_AUTOMATIC);
    PID_SetMode(&pidBL, _PID_MODE_AUTOMATIC);
    PID_SetMode(&pidBR, _PID_MODE_AUTOMATIC);
    
    // Устанавливаем время семплирования
    PID_SetSampleTime(&pidFL, PID_UPDATE_PERIOD);
    PID_SetSampleTime(&pidFR, PID_UPDATE_PERIOD);
    PID_SetSampleTime(&pidBL, PID_UPDATE_PERIOD);
    PID_SetSampleTime(&pidBR, PID_UPDATE_PERIOD);
    
    pidInitialized = true;
    printf("=== PID CONTROLLERS INITIALIZED SUCCESSFULLY ===\r\n");
    printf("PID Parameters: Kp=%.2f, Ki=%.2f, Kd=%.2f\r\n", KP, KI, KD);
    printf("Output limits: ±15, PWM range: %d-%d\r\n", MOTOR_DEADZONE_MIN, MOTOR_DEADZONE_MAX);
    

    }

    uint32_t CompensateDeadzone(double pidOutput) { double absOutput = fabs(pidOutput); if (absOutput < 1.0) return 0; if (absOutput > 15.0) absOutput = 15.0;

    // Линейное масштабирование без сложных расчетов
    return (uint32_t)(absOutput * (35.0 / 15.0));
    

    }

    // Добавляем функцию для детального логирования PID void LogPIDState(const char* wheelName, PID_TypeDef* pid, double setpoint, double input, double output) { printf("SETPOINT: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", setpointFL, setpointFR, setpointBL, setpointBR); printf("CURRENT: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR); printf("PID_OUT: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR); } /* USER CODE END 0 */

    /* USER CODE BEGIN 2 */ printf("Hi, from machine! \r\n");

    // Инициализация переменных перед PID setpointFL = setpointFR = setpointBL = setpointBR = 0.0; currentSpeedFL = currentSpeedFR = currentSpeedBL = currentSpeedBR = 0.0; pidOutputFL = pidOutputFR = pidOutputBL = pidOutputBR = 0.0;

    InitializePID(); /* USER CODE END 2 */

    /* USER CODE BEGIN 4 / void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { for (int i = 0; i < 4; i++) { if (GPIO_Pin == sensors[i].pin) { ((sensors[i].counter))++; // Добавляем логирование энкодеров при отладке if (pidDebugEnabled && (*(sensors[i].counter)) % 100 == 0) { printf("ENC_%d: %ld\r\n", i, (sensors[i].counter)); } break; } } } / USER CODE END 4 */

    void Start_Motor_Task(void argument) { / USER CODE BEGIN Start_Motor_Task */ #define PACKET_TIMEOUT 50 #define JOYSTICK_UPDATE_RATE 20 #define PWM_UPDATE_INTERVAL 50

    // Инициализация GPIO и PWM HAL_GPIO_WritePin(Mot_A2_GPIO_Port, Mot_A2_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOD, Mot_D2_Pin | Mot_C2_Pin | Mot_B2_Pin, GPIO_PIN_RESET);

    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);

    // uint32_t now = 0; Ringbuf_Init(); Wheels_Init();

    // Инициализация очереди для пакетов packetQueueHandle = osMessageQueueNew(PACKET_QUEUE_SIZE, sizeof(PacketMessage), NULL); if (packetQueueHandle == NULL) { while (1) { printf("Queue init failed!\r\n"); osDelay(1000); } }

    // typedef struct { // uint8_t buttonId; // uint8_t stableState; // uint32_t lastStableEventTime; // } ButtonState;

    // ButtonState buttons[MAX_BUTTONS] = {0}; // for (int i = 0; i < MAX_BUTTONS; i++) { // buttons[i].buttonId = i; // }

    uint32_t lastPwmUpdateTime = 0; bool pwmRunning = false; static uint32_t lastPwmValues[4] = {0};

    // Инициализация массивов PWM memset(pwmData_CH1, 0, sizeof(pwmData_CH1)); memset(pwmData_CH2, 0, sizeof(pwmData_CH2)); memset(pwmData_CH3, 0, sizeof(pwmData_CH3)); memset(pwmData_CH4, 0, sizeof(pwmData_CH4));

    // Запуск PWM с DMA HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_1, (uint32_t*) pwmData_CH1, 10); HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_2, (uint32_t*) pwmData_CH2, 10); HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_3, (uint32_t*) pwmData_CH3, 10); HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_4, (uint32_t*) pwmData_CH4, 10);

    pwmRunning = true; Wheels_Stop();

    // ВАЖНО: Начинаем с прямого режима currentControlMode = CONTROL_MODE_DIRECT; printf("Started in DIRECT MODE\r\n"); /* Infinite loop */ for (;;) { // Обработка UART данных if (Tail != Head) { osMutexAcquire(myMutexHandle, osWaitForever); // now = HAL_GetTick();

                uint16_t bytesToProcess = (Head >= Tail) ? (Head - Tail) : (MainBuf_SIZE - Tail + Head);
                uint16_t maxBytes = 16;
                if (bytesToProcess > maxBytes)
                    bytesToProcess = maxBytes;
    
                uint8_t tempBuffer[16];
                uint16_t tempTail = Tail;
    
                for (uint16_t i = 0; i < bytesToProcess; i++) {
                    tempBuffer[i] = MainBuf[tempTail];
                    tempTail = (tempTail + 1) % MainBuf_SIZE;
                }
    
                Tail = tempTail;
                osMutexRelease(myMutexHandle);
    
                for (uint16_t i = 0; i < bytesToProcess; i++) {
                    UART_ProcessByte(tempBuffer[i]);
                }
            }
    
            // Обновление PWM
            if (pwmRunning && (HAL_GetTick() - lastPwmUpdateTime) >= PWM_UPDATE_INTERVAL) {
                lastPwmUpdateTime = HAL_GetTick();
    
                uint32_t newValues[4] = {0};
                bool motorsActive = false;
    
                // УЛУЧШЕННАЯ логика выбора режима
                if (currentControlMode == CONTROL_MODE_PID) {
                    if (targetSpeeds.isActive) {
                        motorsActive = true;
                        newValues[0] = CompensateDeadzone(pidOutputFL);
                        newValues[1] = CompensateDeadzone(pidOutputFR);
                        newValues[2] = CompensateDeadzone(pidOutputBL);
                        newValues[3] = CompensateDeadzone(pidOutputBR);
    
                        HAL_GPIO_WritePin(Mot_A2_GPIO_Port, Mot_A2_Pin, (pidOutputFL < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
                        HAL_GPIO_WritePin(GPIOD, Mot_B2_Pin, (pidOutputFR < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
                        HAL_GPIO_WritePin(GPIOD, Mot_C2_Pin, (pidOutputBL < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
                        HAL_GPIO_WritePin(GPIOD, Mot_D2_Pin, (pidOutputBR < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET);
                    }
                } else {
                    MotorsState currentState = Wheels_Get_State();
                    if (currentState.isActive) {
                        motorsActive = true;
                        newValues[0] = currentState.powerFL;
                        newValues[1] = currentState.powerFR;
                        newValues[2] = currentState.powerBL;
                        newValues[3] = currentState.powerBR;
                    }
                }
    
                // Проверяем изменились ли значения
                bool valuesChanged = false;
                for (int i = 0; i < 4; i++) {
                    if (lastPwmValues[i] != newValues[i]) {
                        valuesChanged = true;
                        lastPwmValues[i] = newValues[i];
                    }
                }
    
                // Обновляем PWM значения
                for (int i = 0; i < 10; i++) {
                    pwmData_CH1[i] = newValues[0];
                    pwmData_CH2[i] = newValues[1];
                    pwmData_CH3[i] = newValues[2];
                    pwmData_CH4[i] = newValues[3];
                }
    
                // УПРОЩЕННОЕ логирование для избежания переполнения буфера printf
                if (valuesChanged) {
                    if (currentControlMode == CONTROL_MODE_PID) {
                        printf("[PID] PWM: %lu,%lu,%lu,%lu\r\n", newValues[0], newValues[1], newValues[2], newValues[3]);
                    } else {
                        printf("[DIR] P=%d PWM: %lu\r\n", joystickPower, newValues[0]);
                    }
                }
            }
            osDelay(1);
    }
    /* USER CODE END Start_Motor_Task */
    

    }

    void Start_PID_Task(void argument) { / USER CODE BEGIN Start_PID_Task */ uint32_t lastUpdateTime = 0; uint32_t lastLogTime = 0; const uint32_t LOG_INTERVAL = 15000; // Увеличил интервал логирования

    printf("=== PID TASK STARTED ===\r\n");
    printf("Update period: %d ms\r\n", PID_UPDATE_PERIOD);
    /* Infinite loop */
    

    for(;;) { uint32_t currentTime = HAL_GetTick();

      // Работаем только в режиме PID и по истечении периода обновления
    	if (currentControlMode == CONTROL_MODE_PID && currentTime - lastUpdateTime >= PID_UPDATE_PERIOD) {
    
          // Чтение текущих значений энкодеров с защитой от прерываний
          int32_t currentEncA, currentEncB, currentEncC, currentEncD;
          osMutexAcquire(myMutexHandle, osWaitForever);
          currentEncA = encA_count;
          currentEncB = encB_count;
          currentEncC = encC_count;
          currentEncD = encD_count;
          osMutexRelease(myMutexHandle);
    
          // Вычисление изменений
          int32_t deltaEncA = currentEncA - lastEncA;
          int32_t deltaEncB = currentEncB - lastEncB;
          int32_t deltaEncC = currentEncC - lastEncC;
          int32_t deltaEncD = currentEncD - lastEncD;
    
          // Обновление предыдущих значений
          lastEncA = currentEncA;
          lastEncB = currentEncB;
          lastEncC = currentEncC;
          lastEncD = currentEncD;
    
    		if (targetSpeeds.isActive && pidDebugEnabled) {
    		    static uint32_t debugCounter = 0;
    		    if (++debugCounter % 100 == 0) {  // Каждые 5 секунд при 50ms периоде
    		        printf("PID_DEBUG: ENC_DELTA: A=%ld B=%ld C=%ld D=%ld\r\n",
    		               deltaEncA, deltaEncB, deltaEncC, deltaEncD);
    		        printf("PID_DEBUG: SPEEDS: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n",
    		               currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR);
    		        printf("PID_DEBUG: OUTPUTS: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n",
    		               pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR);
    		    }
    		}
    
          // Расчет скоростей
    

    // double timeFactor = 1000.0f / PID_UPDATE_PERIOD; // double ticksToSpeedFactor = 0.2f; currentSpeedFL = (double)deltaEncA * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0); currentSpeedFR = (double)deltaEncB * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0); currentSpeedBL = (double)deltaEncC * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0); currentSpeedBR = (double)deltaEncD * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0);

          // Установка setpoint'ов
          if (targetSpeeds.isActive) {
              static double rampSpeed = 0;
              rampSpeed += 0.2f;
              if(rampSpeed > 1.0f) rampSpeed = 1.0f;
    
              setpointFL = targetSpeeds.speedFL * rampSpeed;
              setpointFR = targetSpeeds.speedFR * rampSpeed;
              setpointBL = targetSpeeds.speedBL * rampSpeed;
              setpointBR = targetSpeeds.speedBR * rampSpeed;
          } else {
              setpointFL = setpointFR = setpointBL = setpointBR = 0;
          }
    
          // Проверка необходимости вычислений
          bool shouldCompute = targetSpeeds.isActive ||
                             (fabs(currentSpeedFL) > 0.1 || fabs(currentSpeedFR) > 0.1 ||
                              fabs(currentSpeedBL) > 0.1 || fabs(currentSpeedBR) > 0.1);
    
          if (shouldCompute) {
              bool computeFL = PID_Compute(&pidFL);
              bool computeFR = PID_Compute(&pidFR);
              bool computeBL = PID_Compute(&pidBL);
              bool computeBR = PID_Compute(&pidBR);
              printf("PID_COMPUTE: [%d,%d,%d,%d] Ticks: %ld,%ld,%ld,%ld\r\n",
                      computeFL, computeFR, computeBL, computeBR,
                      deltaEncA, deltaEncB, deltaEncC, deltaEncD);
    
              printf("PID_VALUES: SP=[%.1f,%.1f,%.1f,%.1f] IN=[%.1f,%.1f,%.1f,%.1f] OUT=[%.1f,%.1f,%.1f,%.1f]\r\n",
                      setpointFL, setpointFR, setpointBL, setpointBR,
                      currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR,
                      pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR);
    
              // ЛОГИРУЕМ ТОЛЬКО РАЗ В 5 СЕКУНД И ТОЛЬКО ЕСЛИ ЕСТЬ АКТИВНОСТЬ
              static uint32_t lastQuickLog = 0;
              if (targetSpeeds.isActive && (currentTime - lastQuickLog) > 5000) {
    				printf("PID: SP=%.1f CUR=%.1f OUT=%.1f\r\n", setpointFL, currentSpeedFL, pidOutputFL);
                  lastQuickLog = currentTime;
              }
          } else {
              // Пропускаем цикл вычислений
              osDelay(1);
              continue;
          }
    
          lastUpdateTime = currentTime;
          pidLastComputeTime = currentTime;
      }
    
      // Периодическое логирование состояния
      if (pidDebugEnabled && currentTime - lastLogTime >= LOG_INTERVAL) {
          lastLogTime = currentTime;
    
    		printf("\n=== PID STATUS REPORT ===\r\n");
    		printf("Control Mode: %s\r\n", (currentControlMode == CONTROL_MODE_PID) ? "PID" : "DIRECT");
    		printf("Target Active: %s\r\n", targetSpeeds.isActive ? "YES" : "NO");
    		printf("Last Compute: %lu ms ago\r\n", currentTime - pidLastComputeTime);
    		printf("Encoder Values: A=%ld B=%ld C=%ld D=%ld\r\n", encA_count, encB_count, encC_count, encD_count);
    		printf("=========================\r\n");
      }
      osDelay(1);
    

    } /* USER CODE END Start_PID_Task */ }

    void Start_Pkt_Proc_Task(void argument) { / USER CODE BEGIN Start_Pkt_Proc_Task / osDelay(100); PacketMessage msg; // uint32_t lastJoystickUpdateTime = 0; // uint32_t joystickTimeout = 500; // Таймаут в мс - если не получали команд от джойстика за это время, останавливаем моторы / Infinite loop */ for (;;) { // uint32_t currentTime = HAL_GetTick(); // Проверка таймаута джойстика - если давно не получали команд, останавливаем моторы // if ((currentTime - lastJoystickUpdateTime) > joystickTimeout && joystickPower != 0) { // // Останавливаем моторы, если последняя команда была не остановка // Wheels_Stop(); // joystickPower = 0; // joystickDir = 'F'; // printf("[AUTO STOP] Joystick timeout - motors stopped\r\n"); // } if (osMessageQueueGet(packetQueueHandle, &msg, NULL, 10) == osOK) { switch (msg.type) { case PACKET_TYPE_BUTTON: if (msg.data.button.id < MAX_BUTTONS) { uint8_t buttonId = msg.data.button.id; uint8_t newState = (msg.data.button.state == 0x7F) ? 1 : 0;

                        if (buttonId == 4 && newState == 1) { // STOP
                            printf("=== EMERGENCY STOP PRESSED ===\r\n");
                            Wheels_Stop();
                            joystickPower = 0;
                            joystickDir = 'F';
                            targetSpeeds.isActive = false;
                            targetSpeeds.speedFL = 0;
                            targetSpeeds.speedFR = 0;
                            targetSpeeds.speedBL = 0;
                            targetSpeeds.speedBR = 0;
                            printf("All motors stopped, PID targets cleared\r\n");
    
                        } else if (buttonId == 5 && newState == 1) { // Переключение режимов
                            printf("=== MODE SWITCH PRESSED ===\r\n");
                            if (currentControlMode == CONTROL_MODE_DIRECT) {
                                // Сначала останавливаем моторы
                                Wheels_Stop();
                                osDelay(100); // Увеличил задержку
    
                                currentControlMode = CONTROL_MODE_PID;
                                printf("SWITCHED TO: PID MODE\r\n");
                            } else {
                                // Сначала останавливаем PID
                                targetSpeeds.isActive = false;
                                targetSpeeds.speedFL = 0;
                                targetSpeeds.speedFR = 0;
                                targetSpeeds.speedBL = 0;
                                targetSpeeds.speedBR = 0;
                                osDelay(100); // Увеличил задержку
    
                                currentControlMode = CONTROL_MODE_DIRECT;
                                printf("SWITCHED TO: DIRECT MODE\r\n");
                            }
    
                        } else if (buttonId == 6 && newState == 1) {
                            pidDebugEnabled = !pidDebugEnabled;
                            printf("PID Debug: %s\r\n", pidDebugEnabled ? "ENABLED" : "DISABLED");
                        }
                    }
                    break;
    
                case PACKET_TYPE_JOYSTICK: {
                    int8_t x = (int8_t)(msg.data.joystick.x - 127);
                    int8_t y = (int8_t)(msg.data.joystick.y - 127);
                    uint8_t power = msg.data.joystick.power;
    
                    // ТОЛЬКО ОСНОВНАЯ ИНФОРМАЦИЯ
                    printf("JS: x=%d y=%d p=%d\r\n", x, y, power);
    

    // lastJoystickUpdateTime = currentTime; joystickPower = power; joystickDir = (y >= 0) ? 'F' : 'R';

                    if (power <= 100) {
                        if (power == 0) {
                            Wheels_Stop();
                            targetSpeeds.isActive = false;
                            targetSpeeds.speedFL = 0;
                            targetSpeeds.speedFR = 0;
                            targetSpeeds.speedBL = 0;
                            targetSpeeds.speedBR = 0;
                        } else {
                        	if (currentControlMode == CONTROL_MODE_PID) {
                        	    if (power > 30) {  // Уменьшаем максимальную мощность с 40 до 30
                        	        power = 30;
                        	    }
                        	    targetSpeeds.isActive = true;
    
                        	    // ИСПРАВЛЕНИЕ: Еще больше уменьшаем speedScale
                        	    float speedScale = 3.0f; // Было 10.0f - уменьшаем до 3.0f
                        	    float baseSpeed = (power / 100.0f) * speedScale;
    
                        	    float normY = y / 127.0f;
                        	    float normX = x / 127.0f;
    
                        	    float forwardSpeed = normY * baseSpeed;
                        	    float turnSpeed = normX * baseSpeed * 0.5f;  // Увеличиваем коэффициент поворота
    
                        	    targetSpeeds.speedFL = forwardSpeed + turnSpeed;
                        	    targetSpeeds.speedFR = forwardSpeed - turnSpeed;
                        	    targetSpeeds.speedBL = forwardSpeed + turnSpeed;
                        	    targetSpeeds.speedBR = forwardSpeed - turnSpeed;
    
                        	    printf("PID_TGT: %.1f,%.1f,%.1f,%.1f\r\n",
                        	           targetSpeeds.speedFL, targetSpeeds.speedFR,
                        	           targetSpeeds.speedBL, targetSpeeds.speedBR);
                        	} else {
                                Wheels_Process_Joystick(x, y, power);
                            }
                        }
                    }
                }
                    break;
                }
            }
            osDelay(1);
    }
    /* USER CODE END Start_Pkt_Proc_Task */
    

    }

  2. Mistral Assistant

    Отлично, код получен! Теперь задавай свой вопрос, и я постараюсь помочь.

    Что я вижу в коде:

    1. Система управления моторами с поддержкой двух режимов:

      • CONTROL_MODE_DIRECT (прямое управление через джойстик)
      • CONTROL_MODE_PID (регулирование скорости через PID-контроллеры)
    2. Обработка UART-данных (пакеты от джойстика и кнопок)

    3. PID-регуляторы для каждого колеса с настройками:

      • KP = 1.0, KI = 0.05, KD = 0.01
      • Ограничение выхода: ±15 (с масштабированием до 0–35 для PWM)
    4. Энкодеры для измерения скорости вращения колес

    5. Задачи FreeRTOS:

      • Start_Motor_Task — управление моторами
      • Start_PID_Task — вычисление PID
      • Start_Pkt_Proc_Task — обработка пакетов от джойстика

    Возможные вопросы, которые могут возникнуть:

    • Как работает переключение между режимами DIRECT и PID?
    • Почему PID-регуляторы нестабильны или не работают?
    • Как правильно настроить коэффициенты PID?
    • Почему моторы не реагируют на команды?
    • Как улучшить обработку данных от энкодеров?

    Задавай свой вопрос, и я помогу разобраться! 🚀

Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.